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ABSTRACT 

Existing kinematics models for humans cannot simulate movement beyond 
geometric constraints. On the other hand, complex dynamics models are computationally 
expensive for real time computer graphics applications in Virtual Environments(VE). To 
be able to create a more realistic, real time, and computationally efficient human model, a 
simple dynamic model needs to be developed. 

The approach taken in this thesis was to develop a single rigid body dynamic human 
model with massless legs. Instead of a Lagrangian model, which complicates the 
calculations exponentially as the complexity of the system increases, the Newton-Euler 
method was chosen to derive system differential equations. Linear state feedback was used 
for postural control. As part of this research, a previous realistic looking human model is 
further developed. 

The major conclusion of this thesis is that a single rigid body dynamic model can 
be used for simulation of postural control. The simulation results contained in this thesis 
show that such a modeling technique could be used to cause a detailed kinematic 
representation of a human figure to move in a smooth and realistic way without resorting 
to complexity of a multi-link dynamic model. 
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I. INTRODUCTION 



A. MOTIVATION 

There is an increasing demand for realistic virtual environments (VE). One of the 
main branches of research in VE is human motion simulation. Kinematics and dynamics 
are the two disciplines which are used to create physically based mathematical models for 
human motion simulation. Kinematic models are quite simple to implement and 
computationally inexpensive in comparison to dynamic models. However, they are limited 
to simulation of the geometric constraints of the body parts of the human. On the other 
hand, dynamic models introduce additional physical properties of the body parts to the 
simulation, such as mass and moment of inertia, which provides more realism. It is possible 
to simulate human motion realistically with detailed dynamic models. However, the cost of 
this realism is a high degree of computational complexity. When more detailed models are 
chosen, the response time of the simulated human model increases. On the other hand, 
reducing latency to a minimum amount of time is a important requirement for virtual 
environments in which humans can interact. This places limits on the complexity of the 
model which can be used in a particular situation. 

B. GOALS 

The purpose of this thesis is to develop a single rigid body dynamic human model 
with massless legs to illustrate that such a model can simulate human motion more 
smoothly and more realistically than kinematic models, and without resorting to the 
complexity of multi-link dynamic models. The Newton-Euler method is chosen instead of 
the Lagrangian method, since the complexity of the latter grows exponentially when the 
degrees of freedom increase. 
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C. ORGANIZATION 



Chapter II of this thesis provides background information and reviews previous 
work relating to the area of kinematic and dynamic methods to simulate human motion. In 
this chapter, articulated rigid body dynamics, postural control, and control of stepping are 
also investigated in addition to single rigid body dynamics. Chapter III provides an 
overview of the problem statement for this thesis and discusses mathematical modeling of 
postural control of a single rigid body with an attached massless leg. Chapter IV introduces 
the developed kinematic and dynamic computer models. Chapter V presents results and 
conclusions about the work completed. The last chapter. Chapter VI, discusses 
recommendations for future enhancements and further research. 
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II. SURVEY OF PREVIOUS WORK 



A. INTRODUCTION 

Mathematical models which define human motion in detail are complex and quite 
nonlinear. This provides a motivation to start with simple models at a high level of 
abstraction. Models which ignore the dynamic properties of body parts are called kinematic 
models. There are also models which consider only the torso to have dynamic properties. 
More complex models introduce mass and moment of inertia for more than one body part. 
The more dynamic properties are added, the higher becomes the complexity and non- 
linearity of the resulting model. 

B. KINEMATIC MODELS 

Kinematics is concerned with motion without considering the forces and torques 
that cause it. It establishes relations between position, velocity, acceleration, and higher 
order derivatives of position variables. 

The human body can be thought as a set of connected body parts (links). Each body 
part (link) has four link parameters to be defined: link length, {a), link twist ((X), link 

offset(ri) and, joint angle(0). There exists a coordinate frame attached to each link 
[CRAI89]. 

Manipulator kinematics investigates the transformation from frame to frame as the 
body articulates. Forward kinematics show how to compute the position and orientation of 
the end-effector according to link parameters. On the other hand, inverse kinematics solves 
for link parameters when the position and orientation of the end-effector is given. 

For the human body, it can be assumed that the joint angle is the only variable link 
parameter. Forward kinematics can be used to solve for the position and orientation of each 
body part as joint angles are given. However, this method is difficult for the animator. The 
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position and orientation of each body part can also be computed with the position and 
orientation of the end-effector as an input by using inverse kinematics. Even though the 
second method introduces more complexity, in this approach the animator is able to define 
the path for the end-effector. 

C. SINGLE RIGID BODY DYNAMICS 

An advanced approach to human motion simulation introduces dynamics. There are 
three most often used methods: free body method with hard constraints, free body method 
with soft constraints, and generalized coordinates with Lagrangian[MCGH79]. 

1. Free Body Method with Hard Constraints 

This method looks at each body part as a separate free element, under the influences 
of joint torques, gravity, and reaction forces and moments. The simplest possible dynamical 
model for human motion is an inverted pendulum which idealizes the entire body as a 
single rigid link with upright posture maintained by ankle torque. 




Figure 1 : Inverted Pendulum 
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In human motion, joint torques are created by muscular contraction. The origin of 
reaction forces and moments is forces from other limbs or the ground. The force and torque 
equations for the inverted pendulum are [MCGH79] 



mx = F ^ 


(eq.2.1) 


my = Fy-mg 


(eq. 2.2) 


76 = F^/sin6-F^/cos0 + M 


(eq. 2.3) 



where / is the distance from the coordinate origin to the center of gravity, 1 is moment of 
inertia about the center of gravity, F ^ , F ^ are ground reaction forces, and M is the joint 



control torque which is computed by some control law 
M = u{x, t). 

In this equation, x is the system state vector and t is time. 


(eq. 2.4) 



After expressing reaction forces and moments, constraint equations are derived 
according to the geometry of the system, and differentiated twice. Constraint equations for 
this example are 



X = /cosO 

and 


(eq. 2.5) 


y = /sin9 
with second derivatives 


(eq. 2.6) 


.2 

X = -9/sin0-0 /cos0 
and 


(eq. 2.7) 


.2 

y = 0/COS0-0 Zsin0 


(eq. 2.8) 
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The five equations above can be solved numerically or analytically to determine the 



unknowns: x, y , 0, F , F . The matrix form of the equations is 

X y 



m 0 0 -1 0 

0 m 0 0 -1 

0 0 / -/sinO /cos 

1 0 /sinO 0 0 

0 1 -/cosO 0 0 





X 




0 




y 




-mg 


) 


0 


— 


M 








-0^/cos0 


- 


fy. 




.2 

-0 /sinO 



(eq. 2.9) 



The behavior of the pendulum can also be expressed with a 3x3 matrix 
multiplication by analytically eliminating x and y. The 3x3 matrix equation is 
[MCGH79b] 



I -/sinO /cos 
m/sinO 1 0 

-m/cosO 0 1 



)] 


0 






F 


— 




X 






F 











M 

.2 

-mO /cosO 
.2 



(eq.2.10) 



-mO I sinQ + mg 

Since this system has only a single degree of freedom, a suitable state vector is 



X = 



' 0 ^ 

, 0 > 



(eq. 2.11) 



so 



X = 



= 




lej 


X^ 

\ 



(eq. 2.12) 



For simulating this model, numerical integration can be used to compute x{t) for 
any given Jc(0) . The value of 0 (or X ^ ) is calculated by the system dynamic equations. It 
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has been found in the work of this thesis that the 3x3 matrix solution is approximately two 
times faster than the 5x5 one. 

2. Free Body Method with Soft Constraints 

A more realistic approach is to consider the connection between limb segments as 
not rigid. In the soft constraint method, the constraint equations are replaced with functions 
defined by joint variables and their derivatives, which are used to compute constraint forces 
with some gain values. This increases the order of state equation by a factor of three in the 
planar case, and by a factor of six in the three dimensional case, but avoids matrix 
inversion. 

The previous inverted pendulum model becomes as shown in Figure 2 for the soft 
constraint method. 




Figure 2; Inverted Pendulum with Soft Constrains 
Instead of Eq. 2.5 and Eq. 2.6, the soft constraint equations are[MCGH79] 



X = x^ + /cos0 


(eq.2.13) 


y = + IsinB 


(eq. 2.14) 



with the corresponding soft constraint forces 



7 



f^x = - Vfc - '^x^b 



(eq.2.15) 



F 



>‘yyb - Vfc 



(eq. 2.16) 



The state vector for the numerical solution is 



X = (0, 



(eq. 2.17) 



After obtaining the constraint forces by using the state vector values, Eq 2.1, Eq 2.2, and 
Eq 2.3 can be solved directly for translational and angular accelerations instead of requiring 
matrix inversion. 

3. Generalized Coordinates with Lagrangian 

Another way to derive the system equations is to use the Lagrangian method. This 
method does not compute constraint forces and moments, but instead uses the difference of 
kinetic and potential energies expressed in terms of generalized coordinates. 

The virtual work function, for any generalized coordinate, 0 , is 



where K is the kinetic energy function and V is the potential energy function. The 
differential equations of the system are obtained from [MCGH79b] 



5W = (2e50 



(eq. 2.18) 



The Lagrangian function is 



L = K-V 



(eq. 2.19) 



d dL dL 



(eq. 2.20) 






where q ■ represents the generalized coordinate for each i and Q is the coefficient of 
bq- in the virtual work function. 
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Considering the hard constraint example. Figure 1, the virtual work function is 



bW = M60 

where 6 is the generalized coordinate. The kinetic energy of this system is 

1 2 2-2 

T = -(mx + my + 70 ) 



(eq. 2.21) 



(eq. 2.22) 



or 

T = ^(m/^0 sin0 + m/^0 cos0 + /0 ) 

which simplifies to 

1 2 -2 
T = i(mr + /)0 



(eq. 2.23) 



(eq. 2.24) 



The potential energy of the system is 

V = mg/sin0 

Thus, the Lagrangian function is 



L = ^(m/^ + /)0 -mglsmQ 



(eq. 2.25) 



(eq. 2.26) 



By evaluation the Lagrangian derivatives, the result is that the angulai' acceleration of the 
pendulum is given by 



0 = 



M - mglcosQ 
I + mP' 



(eq. 2.27) 



For a numerical solution, the state vector is 



X = 





(eq. 2.28) 
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The Lagrangian approach is perhaps the most effective one for simple cases. 
However, with an increase in the number of degrees of freedom of the system the 
complexity of the calculations grows exponentially. 

D. ARTICULATED RIGID BODY DYNAMICS 

It is certainly impossible to model the human body with complete realism as a single 
mass rigid body. One can, however, abstract it as a system of rigid bodies connected 
together with rotary joints with control torques acting at each joint. If the motion of each 
limb segment is given, and the forces and torques are to be computed, this problem is called 
the inverse dynamics problem. If the torques are given and the accelerations are to be 
determined, this is called the direct dynamics problem [KOOZ83]. 

There is an algorithm, called Articulated Body(AB), for direct dynamics which 
contains three 0( fl) recursions [MCMI95]. In the first step of this method, which is 
Forward Kinematics, velocity and velocity dependent terms are computed from the base to 
the tip of each serial chain of links. The orientation of i ’s coordinate system with respect 

to t — 1 ’s specified by the rotation matrix [CRAI89], ^R - _ j , can be determined by using 

joint position, q-, which is an element of state vector. The position of t ’s coordinate 

system origin according to i — 1 , ^ ^p - , is a constant. Then the spatial transformation 

matrix is [CRAI89] 



/+ 1 



R: 



0 



i+ 1 



X: = 



(eq. 2.29) 



i + 1 



R: 



/ + 1 



R 




i 
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The spatial transformation matrices, along with ^^ are used to compute velocity 

values of the links by using the relation 
i T 

'’( = '’,-1 (eq.2.30) 

The velocity-dependent bias forces, and the vector of Coriolis and centripetal 

accelerations, of each link are also determined from the base to the tip. Details of this 
calculation can be found in [MCMI95b]. 

In the second step, which is backwards dynamics, /t^- and are computed from 
the tip to the base. The matrix ^ is the 6X6 inertia of links i though N . In other words, 
“the inertia ‘felt’ at the i coordinate system when the joints from / -1- 1 io N are free to 
move”[MCMI95]. The vector pt- is the bias force exerted on fth link, including all 
outboard torques. 

In the final step, joint and link accelerations are computed from the base to the tip 
as Aq = 0 is given and by using the equations 

a' i = 'X,. >|),?; + C; (eq. 2.3 1 ) 

and 

/, = (eq.2.32) 

where /^- is the spatial force exerted onto link i by its inboard link which contains the effect 
of input torque, T and where a' is a six element vector containing the angular 
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acceleration, 6d-, and translational acceleration vectors. Each (j)^- is a six element unit 

vector which specifies the corresponding joint axis. 

Another numerical approach to the recursive direct dynamics is based on solving 
the inverse dynamics problem for a sequence of n + 1 specialized acceleration vectors. In 
general, the inverse dynamic equation can be written as [KOOZ83] 

T = CQ-D (eq. 2.33) 

where the matrix C is given by 

C = B~^J = - ] (eq. 2.34) 

and where each element of vector C is a column vector. Usually, C is not known 
explicitly. However it can be computed with a numerical approach using an inverse 

T 

dynamics model such as that in [CRAI89]. Specifically, for the case 0 = (0, 0, ...) , 
from Eq 2.33 

Tq = -D (eq. 2.35) 

The torque Tq can be computed by using the kinematic and dynamic equations of the 

articulated mechanism. As an example, the model given in [KOOZ83] which is illustrated 
in Figure 3 can be used. 

The kinematic equations of the planar model in Figure 3 are 

.2 

= ^■/_i-(^i_i-^i_l)(9/-isin0-_i + 9/_iCos0._j) 

. 2 

-ti'.(0/sin0^. + 0/ cos0p (eq. 2.36) 

and 
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.2 







Figure 3: Free-body for Typical Link of Serial Open-chain Planar Articulated Mechanism 

[KOOZ83] 

Eq. 2.36 and Eq. 2.37 are used to calculate the translational accelerations of the 
links from the base to the tip. By using these accelerations, joint torques can be computed 
in the same recursive manner, but from out to inward with the following equations: 

F =F +m-X; (eq. 2.38) 

Py. = F^^^ ^ -t- m .y. -I- m.g (eq. 2.39) 

L = ^ 1 + + (eq.2.40) 



-F d;SmQ- + F rf cosG • + J Q- 

I jf ^ I I I I 



(eq. 2.41) 
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For the case 0 = ( 1 , 0, . . . ) , all accelerations except 0 1 would be equal to zero 
which means 

Tj = Cj-Z) (eq. 2.42) 

The joint torque, T ^ , can be calculated with the recursive method, which is described 
above. Then Cj can be calculated as 

Cj = T’j+Z) = T^ — Tq (eq. 2.43) 

With the same logic C- is 

Ci = T.-Tq (eq.2.44) 

Thus, the C matrix can be computed numerically by following these steps n times for a n- 
link system. Then 0 is given by 

0 = C~\T-Tq) (eq. 2.45) 

where T is any arbitrary torque and Tq\s the “equilibrium” torque defined by Eq 2.35. 

3 

Because of the required matrix inversion, the second method has 0{n ) complexity. It has 

3 

been shown [MCMI94], that 0{n ) methods are best for simple serial chains of rigid 
bodies when n <3 , and 0(n ) methods are better for n > 3 . 

E. POSTURAL CONTROL 

It is clearly necessary to begin with modeling stable standing of a human before 
considering control of walking. All the methods derived in the previous sections show how 
to compute joint angle accelerations according to joint control torques. The question is how 
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to formulate control torques. One straightforward way is linear state feedback control 
[MCGH86], 

A suitable state vector for postural control can be defined as 



e 



X = 



e 



(eq. 2.46) 



where 0 is an nxl vector of joint positions and 0 is the nxl vector of joint velocities. The 
corresponding body state equation is[KOOZ83] 

K = f{x) + E{x) T (eq. 2.47) 

In Eq. 2.10 f and, E are in general not available analytically. However, with the 
numerical approach, explained in the previous section, the same equation can be expressed 
as 



X 



0 I 

0 0 



x + 




(T-TJ 



(eq. 2.48) 



where I is the unit matrix. 

Linear feedback systems permit much flexibility to the designer, such as pole 

assignment [KOOZ83]. Since C ^ and Tq are functions of x, the system is not linear. 
However, it can be linearized around 0 for joint angular rates and 90 degrees for joint angles 
for erect body position. Then the linearized body state equation can be written as 

X = Ex +GT (eq. 2.49) 

where F is the linearized system matrix and G is the control distribution matrix 
[KOOZ83]. 

Under the assumption of linear state feedback, the control torques vector will be 
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T — K X (eq. 2.50) 

where K is the gain matrix. If Eq. 2.12 and Eq. 2.13 are combined, the derivative of the 



state vector is then 

X = Fx + GKx = [F+G/ir]x = Hx (eq. 2.51) 

Gain values in K should be chosen so that all eigenvalues of H have negative real parts in 
order to obtain a stable postural control [KOOZ83] [CAMA77], 

F. CONTROL OF STEPPING 

If all the postural control system eigenvalues have negative real parts, the 
articulated mechanism can maintain its upright standing posture. The next step is to add 
stepping to the system. At this point there are two options to choose: static or dynamic 
balancing. “A statically balanced system avoids tipping and ensuing horizontal 
accelerations by keeping the center of mass of the body over the polygon of the support, 
formed by the feet.” [RABI 86 ]. On the other hand, a dynamically balanced system can 
depart from static equilibrium and is permitted to tip and accelerate for short period of time. 
By observing human walking, one can easily say that dynamical balancing needs to be 
chosen for simulating a stepping human. 

A step length control method with dynamic stability was presented by Gubina 
[GUBI74]. Gubina’s model has a single rigid body with two supporting massless legs. The 
system has three degrees of freedom. The state vector for the linearized system is 

^ = (r-rQ, r, 9j, 01 , 02 , 62 )^ (eq. 2.52) 

where 0 1 is the leg angle, 02 is the body attitude, r is the leg length, and Tq is the 
desired leg length. 
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Figure 4: Single Rigid Body Model with Variable Length Massless Supporting Legs 

[GUBI74] 

The input vector for the linear state equations is 
T 

u = {u-^,U2) (eq. 2.53) 

where 

w 2 = F — Fq (eq. 2.54) 

and 

«2 = M-Mq (eq. 2.55) 

In these equations, F is the control force applied along the leg, M is the control torque 
applied at the hip and Fq, Mq are bias force and torque respectively. 

The input vector, u , is used to control leg length, and body attitude and step control 
are used to produce the desired forward motion. The leg length is controlled by u | as 
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M j = /ij (r - Tq) + h 2 r 



(eq. 2.56) 



where h j and 7^2 are gain constants. The body attitude is controlled by «2 ^ 

«2 = /t 5 ( 02 -a) + /ig 62 (eq. 2.57) 

where and are gain constants and OC is the bias term to permit the desired attitude. 

When the initial conditions are assumed as r(0) = Tq, r(0) = 0, 
62 ( 0 ) = 0 , 62 ( 0 ) = 0 , the leg angle is governed by the linearized system equation 



2 

01 6 j = 0 (eq. 2.58) 

where 

/ 2 S 

b = —2— (eq.2.59) 




Figure 5: Stable Gait Function [GUBI74] 

As seen in Figure 5, the leg has a negative angular velocity growth rate for negative 
angles and positive angular velocity growth rate for positive angles. The horizontal dotted 
line which is extended from the right to the left represents the change of supporting leg. In 
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the absence of disturbances, the leg angle can be described with a periodic function, 6 j (0 • 

The existence of such a function provides a “stable gait”. However, disturbances do occur 
in locomotion systems. That is why feedback control is needed. With such a control 
mechanism, the leg angle differential equation becomes 



X 



s 



0 1 


X - 


1 


0 . 


s 


q 



u^m5(t-kT) 



(eq. 2.60) 



where 5 denotes the unit inpulse function and 

T 

= (X 3 ,X 4 ) (eq.2.61) 

It is shown in [GUBI74] that a suitable discrete time control vector for this system is 



u^{k) = h^[x^{kT)-Q^] + h^ 



^0 

x^{kT) - 

''0 



+ e 



0 



(eq. 2.62) 



The angle, 6^ , is the desired total leg angle excursion over one cycle, while Vq is the 
desired forward speed. 

Another foot placement algorithm for controlling forward speed is investigated by 
Raibert[RABI74]. In this work, a one legged hopping machine was developed to 
investigate dynamically balanced running robots. The machine has two main parts: the 
body and the leg which is connected to the body with a hinge. There also exists a hip 
actuator to be able to apply a torque from the body to the leg. 
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Body 




Figure 6: One Legged Hopping Machine with Control Variables [RAEB86] 




► 

Figure 7: Placing The Foot at The Neutral Point [RABI74] 



For each forward speed there is a foot position which causes zero acceleration in the 
direction of motion. This point is called the “neutral point”. Placing the foot behind the 
neutral point accelerates the machine, placing the foot in front of the neutral point 
decelerates it. 
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Acceleration 



Deceleration 





Figure 8: Acceleration and Deceleration of the one Legged Hopping Machine According 

to Neutral Point[RAIB86] 

To place the foot at the neutral point, the control system should extend the leg such 
that [RAIB86] 



^/o " ~Y 



(eq. 2.63) 



where x ^ is the forward displacement of the foot with respect to the center mass, x is 

the forward speed, and T ^ is the duration of the stance phase. An additional displacement 
is needed to correct the errors in the forward speed: 

■^/a ~ (eq. 2.64) 

where Xr is the displacement of the foot from the neutral point, Xj is the desired 

forward speed and is the feedback gain constant. 

By combining the equation Eq. 2.64 and Eq. 2.65, the total foot displacement is 
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(eq. 2.65) 



Xf = — + k^(x-x^) 

Then the required hip angle for the desired velocity is 

rxT 

= 0-arcsin( -^ + J (eq. 2.66) 

where y is the angle between the leg and the body [RAIB86]. 

Another research focused on biped systems was conducted by Troy [TROY96]. He 
investigated locomotion of dynamically balanced biped mechanisms. Different multilink 
planar and spatial biped models were developed by using feedback control for balancing 
and walking. In general, his work extends the results described above while confining 
locomotion to a specified sagittal plane. 

G. SUMMARY 

This chapter provides a survey of previous work relating to modeling and control 
of posture and gait for a walking human. It starts with kinematic modeling and continues 
with single and articulated rigid body dynamic models. One section discusses simulation 
of postural control for erect posture of the body. The last part of the chapter illustrates two 
different stepping control approaches for a given desired forward speed. The next chapter 
of this thesis derives the differential equations of a human model which has two degrees of 
freedom by using the Newton-Euler method. It also discusses postural control for the same 
model. 
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III. DETAILED PROBLEM STATEMENT AND MATHEMATICAL 

FORMULATION 



A. INTRODUCTION 

The model in [GUBI74], Figure 4, is designed to simulate postural and gait control 
with three degrees of freedom. The body attitude and the ankle angle are used for postural 
control and the variable leg length and foot placement as well as periodic stepping are used 
for gait control. The model has one input control torque and one input control force. 
Another similar model, dealing only with postural control, will be introduced in this 
chapter. This model has two degrees of freedom with the same variable angles of the 
previous model. Instead of the input control force, this model, shown in Figure 9, has a 
second input control torque at the ankle. 

B. DESCRIPTION OF THE MODEL 




Figure 9; Single Rigid Body Model with A Constant Length Massless Supporting Leg 
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This model, Figure 9, has a single rigid body with one supporting massless leg. The 



system has two degrees of freedom. The state vector for the system is 

T 



X = (6j, 01, 02, 02) 



(eq.3.1) 



where 0j is the leg angle and 02 is the body attitude. The leg length, r , is constant. 

The input vector for the state equations is 
T 



where 



and 



u = (Wp ^2) 



Wj = Mj 



(eq. 3.2) 



(eq. 3.3) 







(eq. 3.4) 



In these equations, is the control torque applied at the ankle and M 2 is the control 
torque applied at the hip. 

The input vector, u , is used to control leg angle and body attitude. According to 
general linear feedback control 



u 



(mJ 



K(x-Xq) 



(eq. 3.5) 



where Xq is the desired state vector. A special form of this equation, called local feedback, 

has been proposed for postural control [KOOZ83][CAMA77]. In local feedback, each joint 
torque is determined by angle and rotation rate of that Joint only. This decouples control of 
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joints from each other. Specifically, in this form of control, the leg angle is controlled by 



Wj as 



where kn and k. are gain constants and OC 
^1 01 

attitude is controlled by 1 I 2 as 



(eq. 3.6) 

is the desired leg angle. The body 



^0 ^2 3.7) 

where ka and k- are gain constants and is the bias term to permit the desired 
^2 02 ^ 

body attitude. Local control will be examined further in this thesis. 



C. THE LAGRANGIAN VERSION OF THE PROBLEM 

The components of the linear velocity vector of the center of mass of the single rigid 
body can be derived from angular variables as follows; 

y = rOj COS0J + /02COS02 (eq.3.8) 

Z = -r0| sin0j-/02Sin02 (eq. 3.9) 

The kinetic energy of the system is thus 




+ ^/02 (eq.3.10) 

2 ^ 

The potential energy of the system can be expressed as 

V = mg(rcos0 j + /cos02) (eq. 3.11) 

Thus, the Lagrangian function becomes 
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1 • • 2 1 - . 2 

L = -m(r0] COS0J + /02COS02) + -w(-r0] sin0j-/02sin02) 



1 -2 

+ ~ ^^(^cos0j + /COS02) 

The differential equation for the first generalized coordinate, 0 ^ , is 



ddL 

^^301 



3L 

30 J 



= Mj-^2 



3L 

The term, — : — , in equation Eq. 3.12 can be derived as 

30] 



3Z/ 2 • 

— r— = mr 0 ] (cos 0 j )^ + /mr02cos0j COS02 
30 ] 



+ mr 0](sin0j)^ + /mr 02 sin 0 jsin 02 
Then, the first term in the equation Eq. 3.12 is 



ddL 2k . .2 '' 2a2 



dfdd 



^ = mr 0 |(cos 0 j)^ - 2 mr 0 ]COS 0 jSin 0 j 



+ /mr02COS0jCOS02-/mr020] sin 0 j cos 02 

•2 2 ” 2 
- /mr02cos0j sin02 + mr 0 |(sin 0 ) 

2.2 

+ 2 mr 0 ] sin 0 j COS02 + m/r02sin02 sin02 

.2 

+ m/r020] COS0] sin02 + m/r02sin0j cos 09 
And the second term in the equation Eq. 3. 12 is 



(eq.3.12) 



(eq.3.13) 



(eq. 3.14) 



(eq. 3.15) 
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3Z> 2*2 

= -mr 6isin0jCos0j -mr/ 0 | 02 sin 0 jcos 02 



2a2 



+ mr 0]Cos0jSin0j + mr/0j02cos02sin02 



+ mgrsin0, (eq. 3.16) 

By substituting Eq. 3.14 and Eq. 3. 15 into Eq. 3. 1 2, the following equation can be obtained: 
2 - 

mr 0 | + /mr02(sin0j sin02 + cos 0 j cos02) 

.2 

+ /mr02(sin0j cos02 - cos 0 j sin02) 

+ /mr020i(cos0jsin02- sin 0 j cos 02) 

+ /mr 0 ] ©2( sin 0 j cos 02 - cos 0 | sin02) 

-mgrsin0j = M^-M^ (eq. 3.17) 

After applying trigonometric conversion rules, the first differential equation of the model 
can be derived as follows: 

2 ” •• -2 
mr 0| + /mr 02 cos (02 - 0 j ) + /mr 02 sin (0 j - 02 ) 

-mgrsin0j = M^-M 2 (eq. 3.18) 

The differential equation for the second generalized coordinate, 02 , is 
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ddL dL 
dt^Q 302 



= M. 



(eq. 3.19) 



dL 

The term, , equation in Eq. 3.19 can be derived as 

302 



* 2*2 
— ^ = m/rGj COS0J COS02 + m/ 02(cos02) 
300 



+ m/r0i sin0jsin02 + m/^02(sin02)^ + /02 

The time derivative of Eq. 3.20 is 

d 3Z> *• 

— = m/r0i COS 01 COS0O - m/r0i sin0i COS0O 

112 112 



(eq. 3.20) 



• • 2 " 2 

- m/r0]02cos02 sin02 + wi/ 02(cos02) 

2-2 

- 2 ml 02 cos 02 sin 02 + sin 0 j sin02 



+ m/r 0 1 cos 0 j sin 02 + m/r0i02Sin0 ^ cos02 



+ m/^02(sin02)^ + 2m/^02sin02cos02 + /02 

Then the second term in the equation Eq. 3.18 can be derived as 

3L 



30. 



= - m/r02 02 cos 0 j sin 02 + m/r 0 ^ 02 sin 0 | cos 02 



(eq.3.21) 



+ mg /sin 02 



(eq. 3.22) 



By substituting Eq. 3.21 and Eq. 3.22 into Eq. 3.19, the following equation can be obtained: 
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. . . ^ 
m/rOj COS0 j cos ©2 - m/r0j sin 0j cos 02 

• • 2 -- 2 

- m/r 0 1 02COS0J sin 02 + ml 02(cos02) 

2-2 

- 2 ml 02cos02sin02 + m/r 0 | sin 0 j sin02 

.2 

+ m/r0] COS0 j sin02 + m/r0j02sin0j cos02 

2 • * 2 *2 

ml 02 (sin 02 ) + 2 m/^ 02 sin 02 cos 02 + /02 

+ m/r0i02cos0j sin02-m/r0]02sin0j COS02 

+ mg/sin02 = M 2 (eq. 3.23) 

Then, the second differential equation of the model can thus be expressed as follows: 

.2 . 

m/r 0 | cos( 0 | - 02) + m/r 0 1 sin (02 ~ 0 j) 

2 •• 

+ (/ + m/ )02-mg/sin02 = M 2 (eq. 3.24) 

The model in [GUBI74] has an additional degree of freedom comparison to the 
model presented in this thesis which is variable leg length and does not have any ankle 

control torque. If the time derivative values of the leg length variable r are taken out, and 
the ankle torque Mj is added to the dynamic equations in [GUBI74], it is seen that these 
equations are identical with the equations Eq. 3.17 and Eq. 3.23. 
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D. NEWTON-EULER FORMULATION 



As discussed in the previous chapter, if the Lagrangian method is used, an increase 
in the number of degrees of freedom complicates the calculations exponentially for an 
articulated mechanism. Even though the number of variables are not many in this case, by 
considering that future research is likely to investigate more complex models, the Newton- 
Euler method looks as if a better approach for this kind of problems. 

The model, presented in this chapter, can be divided into two parts to simplify the 
problem. These parts are the massless leg and the body itself. 

1. The Newton-Euler Equations of the Massless Leg 




Figure 10: Free Body Diagram for The Massless Leg 
Because the leg has no mass, it is only possible to talk about the static equilibrium 
of this body part. The equilibrium equations can be expressed as 
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n 




i = 1 



and 



n 

= 0 

i = 1 

where M ■, F ■ and r are defined in Figure 10. 

Equation Eq. 3.25 can also be written as 

M^-M^-rF^ = 0 

so the reaction force, F ^ , is given by 



M -M 




2. The Newton-Euler Equations of the Body 




Figure 1 1 : Free Body Diagram of The Body 



(eq.3.25) 



(eq. 3.26) 



(eq. 3.27) 



(eq. 3.28) 
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The dynamic equations of the free body in the z and y directions can be derived as 
mz = - mg + F cos 0 j - F^sin 0 ^ (eq. 3.29) 

and 

my = Fsin0| - F^cos0j (eq. 3.30) 

For the angular motion of the free body, the dynamic equation of the body can be expressed 
as 

702 = M 2 - /F^COs( 02 - 0j) + /Fsin(02 - 0j) (eq. 3.31) 

3. Combining Body and Leg Equations 

As shown in Figure 9, the geometric constraint equations for the model can be 
derived as 

y = rsin0j +/sin02 (eq. 3.32) 

and 

Z = rcos0| + /COS02 • (eq. 3.33) 

The second time derivatives of Eq. 3.30 and Eq. 3.31 are as follows; 

.2 .. .2 

y = r0j COS0J - r0| sin0^ + /02COS02 - /02sin02 (eq. 3.34) 

and 

.2 .. .2 

z = - r0|Sin0^ - r0j COS0| - /02Sin02 - /02COS02 • (eq. 3.35) 

The constraint force F ^ in equation Eq. 3.29 can be eliminated by using equation Eq. 3.26 
resulting in the expression 
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(eq. 3.36) 



/(M, -M«)cos( 0 -, - 0 .) 

/02 = M2 i ^ = 5 - + /Fsin(02-0j) 

By combining Eq. 3.28 and 3.32, the following equation can be derived: 

.2 

m(rd] COS 0 J - r 0 | sin 0 j + /02cos02 - /02sin02) = 

= Fsin 0 j + F^,cos 0 j 

On the other hand, after substituting z with Eq. 3.33, Eq. 3.27 can be written as 

... .2 .. .2 

m(- r 0 ] sin 0 j - r 0 | cos 0 j - /02sin02 - /02cos02) = 

= - mg + Fcos 0 j - F^sin 0 j 

The equation Eq. 3.34 can redefined as 

MM + r)-lM, 

02/-F/sin(02-0j) = cos(02-0j) 

After reorganizing, Eq. 3.35 and Eq. 3.36 become 

0 ]mrcos 0 | + 02m/cos02 - Fsin 0 j = 

• 2 . .2 . -^2 

= mr 0 ] sin 0 j + m/02sin0.;) H cos 0 j 

and 



0 |mrsin 0 ^ + 02m/sin02 + Fcos 0 j = 

.2 -2 M| — M 2 

= -mr 0 j COS 0 j -m /02 COS 02 + mg + sin 0 j 

Equations Eq. 3.37, Eq. 3.38, and Eq. 3.39 can be expressed in matrix form as: 



(eq. 3.37) 



(eq. 3.38) 



(eq. 3.39) 



(eq. 3.40) 



(eq.3.41) 
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0 

mr cos 0^ 
mrsinGj 



/ -/sin(e2-6i) 

m/ cos 02 -sin0j 



mlsinQr 



COS0, 



)1 


ei 




02 


_ 


j 



MM + r)-lM, 

cos (02 - Qj) 

.2 .2 My-M^ 

mrGj sin 0 j + mZ02sin02 + cos 0 ^ 

.2 .2 ^\~^2 
- mr 0 i COS 0 J - m/02COS02 + mg + sin 0 j 



(eq. 3.42) 



An alternative to this analytic formulation of the Newton-Euler formulation is to 
use the recursive approach explained in Chapter II of this thesis. Thus results in more 
complex logic, but in less computation, especially as the number of links in the dynamic 
model increases. 



E. LINEARIZED ANALYSIS FOR CHOOSING GAINS 

As mentioned in the previous sections, this system has two inputs, hip and ankle 
torques. The main purpose of postural control is to determine these input torques in order 
to maintain the upright orientation of the body. The approach taken in this thesis is linear 
state feedback control. The control equations, Eq. 3.5 and Eq. 3.6, are presented in the 
second section of this chapter. For the local control model, the problem is to compute the 

required gain values; ka , k- , ka , k. . 

9, t»2 02 

It is reasonable to drop quadratic velocity components for small motion linearized 

.2 .2 

analysis [MCGH86]. Under this assumption, 0j and 02, components are removed from 
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all equations. The same assumption means that 6j and ©2 can have only small values 



which allows the substitutions 



sin0j = 0j , 


(eq.3.43) 


sin 02 = 02, 


(eq. 3.44) 


COS0J = 1, 


(eq. 3.45) 


and 




COS 02 = 1 . 


(eq. 3.46) 


With these considerations, equation Eq. 3.18 can be rewritten as 




2 •• 

mr 0 i+/wr 02 = wgr0j+Mj-M2. 


(eq. 3.47) 


The linearized version of equation Eq. 3.24 is 




w/r 0 |+(/+m/ )©2 = wg ^/02 + M2 


(eq. 3.48) 



02 can be evaluated from equation Eq. 3.47 as 

2 " 

/W2T01 + Ml —M^—mr 0i 

02 = ^ 7 (eq. 3.49) 

Imr 

If equation Eq. 3.49 is substituted in equation Eq. 3.48, the following equation is obtained 

(w/r)^01 + (/+ m/^)(wgr0j + Mj -M^-mr^Q^) 

2 2 

= rm gl 02 + mlrM2 (eq. 3.50) 

After reorganizing, equation Eq. 3.50, 0 \ can be defined as 

01 = 
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2 

/^m^rg02 - mgr{l + m/^)6| — (/ + + {Imr + I + ml )M 2 



-Im/ 



(eq.3.51) 



The angular acceleration, 0] , can be defined by using equation Eq. 3.47 as 



01 



m 



gr0j + Mj - M 2 - lmr^2 



(eq. 3.52) 



mr 



After substitution of 0| , equation Eq. 3.48 can be rewritten as 
/ •• 2 " 

-(mgr0| + Mj - M 2 - /wr02) + (/+ /w/ )02 =mg/02 + M 2 (eq. 3.53) 



After reorganizing equation Eq. 3.53, 02 can be define as 

02 = 

mglr ^2 ~ ] - ^Mj + (r + /)M^ 

7l 

The general linear state feedback equation is 
X = Ax + Bu 



(eq. 3.54) 



(eq. 3.55) 



The state vector, X , is defined by equation Eq. 3.1 . If the desired values for the angles are 
chosen as zero, the input vector, in equation Eq. 3.5, can be rewritten as 

u = Kx (eq. 3.56) 

After substituting u according to the equation Eq. 3.56, Eq 3.53 can be written as 
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X = (A + 5/:)x 



(eq. 3.57) 



where 



A = 



0 



1 0 0 



2 

mgr(I + ml ) 


0 


2 2 
/ m rg 


, 2 


2 


Imr 




Imr 


0 


0 


0 


-Imgr 


0 


mglr 


Ir 


Ir 



and 



B = 



0 



0 



I + ml -(Imr + I + ml ) 



Imr 

0 

Ir 



Imr 

0 

r + / 
Ir 



(eq. 3.58) 



(eq. 3.59) 



For local feedback, K can be defined as follows, according to the equations Eq. 3.2, Eq. 
3.6 and Eq 3.7 



K = 



-Kr, -K. 0 0 

^1 01 

0 0 -Ka -K. 

^2 02 



(eq. 3.60) 



where desired angle values and 0.2 taken as zero. Then, by using equation Eq. 
3.58, Eq. 3.59 and Eq. 3.60, A + BK can be defined as 
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where 



A + BK = 



0 10 0 
M N O P 
0 0 0 1 
_Q V Y Z\ 



(eq. 3.61) 



M = 



2 2 
mgr(I + ml )-Kq (1 + ml ) 

Imr^ 



(eq. 3.62) 



N = 



-K^y + ml ) 
Imr^ 



(eq. 3.63) 



O = 



2 2 2 
-I m rg + K^ (Imr + 1 + ml ) 

Imr^ 



(eq. 3.64) 



p “ 


02^ 


(eq. 3.65) 




Imr^ 


Q = 


- Imgr + IKq^ 
rl 


(eq. 3.66) 


V = 


rl 


(eq. 3.67) 


Y — 


mglr - Kq (r + /) 


(eq. 3.68) 




rl 


7 — 


-Kyr + l) 


(eq. 3.69) 




rl 
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For a stable upright posture, all roots of the characteristic equation (eigenvalues) 
should have negative real parts [KU095]. The characteristic equation of the closed loop 
system is [KU095] 

|,S-/-(A + BA:)| = 0 (eq. 3.70) 

After substituting the equation Eq. 3.60 the equation, Eq. 3.69 can be rewritten as 



.y -1 0 0 
C D E F 
0 0 y -1 
G H W Y 

i 

where 



(eq.3.71) 



2 2 
- mgr(I + ml ) + Kq (I + ml ) 

C = ^ , (eq.3.72) 

Imr 

K. a + mF) 

D = s + 2 ’ (eq.3.73) 

Imr 



E = 



2 2 2 
/ m rg-K^ (Imr + 1 + ml ) 

Imr^ 



F = 



-K. (Imr + I + ml ) 
02 

Imr^ 



(eq. 3.74) 



(eq.3.75) 



G = 



Imgr — Kq I 

7l 



(eq.3.76) 
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H = 



-K. I 

0i_ 

rl ’ 



(eq. 3.77) 



W = 



-Imgr + ^0 (r + /) 



rl 



(eq. 3.78) 



Y = s + 



rl 



(eq. 3.79) 



If the determinant of in the equation Eq. 3.69 is evaluated, the following polynomial is 
obtained: 

4 2 

s (r Ini) 

s^ilrmK ■ +IK. +K. m^ + r^mK-\ 

\ 02 01 01 02j 



+ {I K^^-i^rti^rg + -r^m^lg 



■e, 



+ r mK^ + rmlKf^ + -Imgr) 



®2 01 02 



^s(K,K^-lmgK.^^K^K,-msrK.) 

+ [m^g^rl-mgrK^^-K^lmg + K^Kf^^ = 0 



(eq. 3.80) 



It is clearly a very hard problem to determine the required gain values in equation Eq. 3.80, 
which result in all roots having negative real parts. Instead, a experimental solution was 
found by trying some gain values on the computer model. It was found that the following 
values produce an upright stable body posture: 
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Kq = 25000 



(eq.3.81) 



K. = 2500 . 

6i 



(eq. 3.82) 



Kf. = 4000 , 

^2 



(eq. 3.83) 



K. = 400 

02 



(eq. 3.84) 



F. SUMMARY 

In this chapter, a new human dynamic model, which is made of a body and a 
massless leg, is introduced. The dynamic differential equations of the model are derived by 
using two different methods: the Lagrangian and the Newton-Euler methods. The last 
section of the chapter discusses the small motion linearized analysis of the system. It is 
explained how the gain constants for the input torques should be chosen. 

The next chapter will explain the implementation of computer models based on the 
knowledge presented in the second and third chapters of this thesis. 
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rv. COMPUTER MODELS 



A. INTRODUCTION 

In the second chapter of this thesis, it is pointed out that there exist two major 
methods to simulate human motion simulation: kinematic and dynamic models. The 
present ch^ter first presents stepping algorithms for a human figure implementation which 
is developed by using a kinematic model. The second chapter also discusses the general 
approaches for dynamic simulation of human motion. Based on this knowledge, the third 
chapter investigates the mathematical representation of a dynamic human model. The 
present chapter also introduces the computer implementations of these models. 

B. KINEMATIC COMPUTER MODEL (DYNAMAN) 




Figure 12: Kinematic Computer Model: Dynaman 






The kinematic model presented in this section is made of fifteen separate body parts 



as shown in Figure 13. 



o DCSl 




15 

16 



Figure 13: The Body Parts and Corresponding Dynamic Coordinate Systems (DCS) of 

Dynaman 

The purpose of the simulation is to create a computer representation of a stepping 
human. Under this requirement, it is clear that the main concern is leg and foot motion. The 
other body parts are synchronized according to the legs. 

Inverse kinematics is chosen for computing leg motion. Inverse kinematic 
equations take the position of the end effector (foot) as the input and computes each joint 
angle of the leg. The points which describe the position of the foot in space for a full gait 
period produce a path. The question is to define this path as a mathematical function of 
time. This function, of course, should be developed with the knowledge of the geometry of 
the environment, such as height of each stair. 

Forward kinematics could have been chosen for the same problem. In that case. 
Joint angles of the leg would be the input to the forward kinematic equations, and the 
position of the end effector would be calculated. For this second case, there should exist a 
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feedback system in order to input the constraints of the environment to the model. A 
collision detection mechanism between the foot and the floor seems to be appropriate to this 
approach [GOET94], 

1. Inverse Kinematic Equations for Three Link Planar Manipulator 




Figure 14: Three Link Planar Manipulator 

The leg of Dynaman can be thought as a three link planar manipulator which is 
made of the upper leg, the lower leg, and the foot. The frames which are attached to the 
links are shown in Figure 14. The general form of the transformation matrix to represent a 

point in the frame I — 1 which is defined in frame i is [CRAI89] 
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(eq.4.1) 



cosQ- -sin0. 0 ^/-i 

_ sin0^cosa^ •_ j cos0^cosa^ _ j -sina^- j -^i^sina^ _ j 

sin0^.sina^ •_ I cos0jSina^_ j cosa^_j d-cosa-_^ 

0 0 0 1 

where a, (X, 0 , and d are the link parameters as defined in the second chapter of this 
thesis. The link parameters for the presented model are defined in Table 1. 
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Table 1. Link Parameters of the Leg [CRAI89] 



In this model, all z- axes are parallel, and all X- axes are in the same plane. That is 



why all the OC^- _ j and d- values are zero. The parameters / j and I 2 are the link lengths 



of the first and second links. The parameter defines the position of the toe point. Since 



it is in the end effector frame, it is not included in the link parameters. 

According to equation Eq. 4.1 and Table 1, the transformation matrices for the 
neighbor links are 



0 

1 



T = 



COS0J -sin0j 0 0 

sin0j COS0J 0 0 

0 0 10 
0 0 0 1 



(eq. 4.2) 
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(eq. 4.3) 



(eq. 4.4) 



The transformation matrix between the first and the last links can be derived by using the 
product 

jT = \t It (eq.4.5) 



If the matrix multiplications are executed after substituting equations Eq. 4.2, Eq. 4.3, and 
Eq. 4.4 into the equation Eq. 4.5, the following transformation matrix will be obtained: 




cos( 9 j + ©2 + 63) -sin( 0 j + ©2 + 63) 0 

sin( 9 j + ©2 + 63) cos( 9 j + 02 + 93) 0 

0 0 1 

9 9 0 



/jCOS0j +/2COS(0J + 03 ) 

/jSin0j + /2sin(0j + 03) 

0 

1 



(eq. 4.6) 

If the method in [CRAI89] is used, the transformation matrix of the end effector 
according to the base link can be defined as 
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(eq. 4.7) 



0 

3 



T = 



cos 6 -sin 9 0 x 
sin6 COS0 0 y 
0 0 10 
0 0 0 1 



where the position components, x and y, and the orientation, 0, of the end effector 
according to the base link are known. The following equations can be derived from 
equations Eq. 4.6 and Eq. 4.7: 



COS0 = COS(0J + 02 + 03 ) 


(eq. 4.8) 


sinO = sin(0j + 02 + 03 ) 


(eq. 4.9) 


X = COS0J + /2cos(0j + 02) 


(eq. 4.10) 


y = /jSinOj + /2sin(0j + 02) 


(eq. 4.11) 



If the squares of equations Eq. 4.10 and Eq 4.11 are added, the following equation is 
obtained: 



2 2 
X + >' 





+ 2/J/2COSO2 



(eq. 4.12) 



In equation Eq. 4.12, the only unknown is 02 and it can be defined as 



02 = acos 



/ 2 2 , 2 , 2 . 

' X +y -/j -^ 2 ' 

2/^/2 



(eq.4.13) 



After having found 02 , equations Eq. 4.10 and 4.11 can be can be written as 



X = /:j COS0J - /:2Sin0j 



(eq. 4.14) 



and 
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= ^jSinGj +^2cos0j 



(eq.4.15) 



where 



= /j + /2COS02 



and 



/^2 — /2SIH02 



(eq. 4.16) 



(eq.4.17) 



After following the steps in [CRAI89], 0 j is defined by the equation 



0j = atan 



g}atan 



fk \ 

k. 

ly 



Then foot angle can then be calculated as 



0^ = 0 — 0j — 02 



(eq. 4.18) 



(eq. 4. 19) 



2. Link Descriptions in the Computer Model 

The computer model has been developed in the IRIS Performer™ application 
environment. This environment allows the programer to create a Dynamic Coordinate 
Systems (DCS). DCS lets the programer to change the transformation of the objects which 
are attached to that DCS node. The method followed in this application is to attach each 
body part to a DCS node and connect all the DCS nodes in a tree structure which creates a 
hierarchical structure. For example, if the upper leg is rotated for some degrees, all the body 
parts under that DCS node, lower leg and foot, follow this rotation. DCS tree structure of 
the whole body is shown in Figure 13 and Figure 15. 
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DCSO 




1 i /\ i 1 



DCS 12 DCS 15 DCS 6 DCS 1 DCS I DCS 4 

11 11 

DCS 13 DCS 16 DCS 8 DCS 9 

Figure 15: Dynamic Coordinate System (DCS) Hierarchy Tree of Dynaman 

The body parts of the Dynaman are taken from an other model developed in 

OpenGL'^ by Will Frey at the Naval Postgraduate School [FREY96). However, while Frey 
used body segment Euler angles relative to an Earth fixed reference system, as described 
above, the work of this thesis is based on joint angles. The coordinates of the polygons, 
taken from the other model, which produces the body parts, are loaded to the application 
by using poly format files. 

3. Stepping Algorithms 

The inverse kinematics model takes position and orientation of the end effector and 
computes each joint angle. Then, the question is to define the path of the end effector (foot) 
which is the input to the inverse kinematics model. Two separate algorithms are developed 
to define the path: stepping forward and stepping upward algorithms. 

a. Stepping Forward Algorithm 

The leg, without bending at the knee, can be assumed as a simple swinging 
bar on a circular path which is shown as F j in Figure 16. The angle between the vertical 
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and the swinging bar is defined as Ot , which takes values between -20 and -t-20 degrees 
during the gait cycle. In reality, the knees are bent during stepping. Larger values of (X 

introduce larger bending angles at the knee. When CL is equal to zero, knee bending angle 
should also be zero. This requirement is introduced by 

Pi = P 2 COS a (eq. 4.20) 

There is a difference between the supporting and the recovery leg motions. While 
the supporting leg always touches the ground, the recovery leg should be moved without 
touching the ground. This requirement is added to the model by using the following 
equation for the recovery leg 

= 0.95^2 (eq.4.21) 




Figure 16: Forward Stepping Algorithm 
No kinematic computation is needed for the arms. They are synchronized 
with the legs. The arm rotation is not exactly the same as leg rotation; a scale factor is 
applied for a more realistic looking arm motion. Another property of the model is the 
rotation of the upper body around the vector described by the general direction of motion. 
This motion is caused by the roll moments which are generated by the nature of biped 
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locomotion since the supporting legs have offset from the center of mass on opposite sides 
and are alternating during each step cycle. 




Figure 17: Upper Body Rotation 

The distance from the upper body to the ground is not constant. When the 
CX parameter has larger values, the whole body gets closer to the ground. This property is 
implemented by moving the whole body vertically with a cosine function of time. 




Figure 18: Change in the Height of the Body During One Gait Cycle 
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The height of the upper body is calculated by 
Height = 0.08 cos (27i/f) 



(eq. 4.22) 



b. Stepping Upward Algorithm 

The stepping up algorithm has a major difference from the forward stepping 
which is caused by the need for increase in body elevation 




Figure 19: The Amount of Elevation For Stepping Up 
The needed elevation for stepping up is 

h = — (/j+ I 2 ) COS P . (eq. 4.23) 

This elevation can only be handled by raising the foot to a higher position in the front. This 
causes the change which is shown in Figure 20. The values of (X change from -10 to +30 
degrees for the gait cycle. 
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Figure 20: Stepping Up Algorithm 
C. DYNAMIC COMPUTER MODELS 

1. Newton-Euler Rigid Body Class 

The Dynamic models in this thesis are developed in ANSI Common Lisp. The main 
class for the dynamic simulation is rigid-body class which was written by Professor 
McGhee. This Lisp code for this class is included as Appendix A of this thesis. This class 
defines a free rigid body in space. The major method of the rigid-body class is update-rigid- 
body which updates the posture vector which includes the position and the orientation of 
the rigid body in an earth coordinate system. This method converts body velocity rates to 
earth velocity rates to update the six element posture vector. Euler integration is used to 
update body velocities by using body velocity growth rates. The body velocity growth rates 
result from applied forces and torques on the body. The linear velocity growth rate is 
computed by [FRAN69] 
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(eq. 4.24) 



where / , / , /. is the force vector applied to the body, («, v, w) is the linear velocity 

ji y Z 



vector, (p,q,r) is the rotational velocity vector, ((j), 6, \}/) is the body orientation 

defined in Euler angles, m is the mass, and g is the gravitational acceleration. All these 
vectors are defined in a body principal axis coordinate system [FRAN69]. The angular 
velocity growth rate is computed by 
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q 

r 



Wyy-l,,)qr^L] 

'yy 



(eq. 4.25) 



where (L, M, A^) is the torque vector applied to the body, I and are mass 

moments of inertia, (p, q, r) is the rotational velocity vector [FRAN69]. In summary, the 
update-rigid-body method calculates the new position and orientation of the free rigid body 
in an earth coordinate system according to the applied forces and torques to the body. An 
other important method is move which takes the displacement components and orientation 
Euler angles as input and moves the body according to the inputs. 
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2. Numerical Integration Methods 

Two different integration methods are used in the dynamic modeling 
implementations: Euler and Huen integration. The Euler integration approach can be 
formalized as [KREY88] 

^n + \ = (®q. 4.26) 

where 

y = t) (eq. 4.27) 

and Af is a constant increment in the independent variable, t. 

The Heun integration formula can be defined with the equation [KREY88] 

■n+\ = + (eq.4.28) 

where 



n 



+ 1 



* = X 



n 



/(4T„, 



t )Al. 

n' 



(eq. 4.29) 



The symbols At and f (x^, t^) have the same definitions for Heun integration as in Euler 
integration. 



3. Dynamic Inverted Pendulum Simulations 

Three different dynamic inverted pendulum models are implemented in this thesis: 

a. A Single Link Single Rigid Body with Newton-Euler 
The second implementation simulates an inverted pendulum by using Newton- 
Euler formulation of the dynamic equations of the system. The constraint forces from the 
dynamic equations and the control torque are the inputs of the rigid-body class. The 3x3 
matrix multiplication form of the system equations is as follows: 
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(eq. 4.30) 
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where all the variables are defined as in Figure 21. 




Figure 21: Inverted Pendulum with Constraint Forces 
The system has only one degree of freedom. A suitable state vector is 

X=[0, 0] (eq.4.31) 

To assume M = 0 , makes the system behave like a natural inverted pendulum 
without control. By using linear state feedback, the control moment can be determined as 



M = 



(eq. 4.32) 



where and K. are control gain variables. Suitable values for and K . can be 

found analytically for small motion linearization of this system because its characteristic 
equation is quadratic. The Lagrangian formulation, Eq 2. 27, is most useful for this purpose. 
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b. Massless Leg and a Single Rigid Body with Lagrangian 

The second implementation simulates a two link structure which is 
presented in the third chapter of this thesis. The dynamic equations of the system are 
defined with Eq. 3.18 and Eq. 3.24 which are derived by using Lagrangian method. The 
linear state feedback control equations are given by the equations Eq. 3.6 and Eq. 3.7. The 
state vector is defined as Eq. 3.1. 

c. Massless Leg and a Single Rigid Body with Newton-Euler 

The third implementation simulates the same two link structure which is 
simulated in the second simulation. However the Newton-Euler method is used to derive 
the dynamic equation instead of the Lagrangian. The 3x3 matrix form of the dynamic 
equations are given by Eq. 3.42. The linear state feedback control equations and the state 
vector are defined as the same as in the Lagrangian version of the simulation. 

D. SUMMARY 

This chapter presents forward and upward kinematic stepping algorithms, of an 

human model which is developed at the IRIS Performer™ application environment by 
using C+-(-. The second part of the chapter explains three different implementations to 
simulate various inverted pendulum models developed in Lisp. The next chapter discuses 
the results of these simulations. 
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V. RESULTS OF COMPUTER SIMULATIONS 



A. INTRODUCTION 

In this chapter, the results of the dynamic simulations are presented. The second 
part of the chapter contains frames from the kinematic simulation of Dynaman which show 
the model stepping forward and upward. 

B. DYNAMIC SIMULATIONS 

1. A Single Link Single Rigid Body with Nevvton-Euler Method 

Figure 22, Figure 23, and Figure 24 show the behavior of the inverted pendulum 
with a control torque at the pivot point. The dynamic equations of the system are derived 

by using the Newton-Euler Method. The mass of the rigid body,m, is 100 lb. The body 

rotary inertia, /, is 900 lb. ft.^. The length of the inverted pendulum, /, is 3 ft. The 

gravitational acceleration, g , is 32.2185 ft. / sec.^. The gain values for the control torque 
are 

Kq = 10000 (eq.5.1) 

K. = 2000 (eq.5.2) 

0 

The initial state vector is 

x=(l,0) (eq.5.3) 

and the state vector in steady state is 

JC=(0,0) (eq.5.4) 

Figure 25 shows inverted pendulum orientation change in time. Euler integration 
with a time step of 0.02 seconds was used for the results shown in Figure 22-25. 
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Figure 22: Initial Position and Orientation of the Inverted Pendulum 




Figure 23: Inverted Pendulum Moving to the Upright Orientation 
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Figure 24: Inverted Pendulum After Steady State 




Figure 25: Body Attitude Response of the Inverted Pendulum 
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2. Massless Leg and a Single Rigid Body with Lagrangian Method 
Figure 26, Figure 27, Figure 28, and Figure 29 show the behavior of the two link 
inverted pendulum with control torques at the hip and at the ankle. The z axis of the body 
is drawn to be able to observe the body attitude and the leg angle separately. The dynamic 
equations of the system is derived by using the Lagrangian Method. The mass of the rigid 

body,m , is 100 lb. The body rotary inertia, /, is 100 lb. ft.^. The length of the leg, r, is 3 
ft. The length of the rigid body, I , is 0.5 ft. The gravitational acceleration, g , is 32.2 ft. / 
sec. . The gain values for the control torques are 



= 25000 (eq.5.5) 

K- = 2500 (eq. 5.6) 

01 

= 40000 (eq.5.7) 

K. = 400 (eq.5.8) 

02 

The initial state vector is 

X = (0.5,0, 1,0) (eq. 5.9) 

and the state vector in steady state is 

JC = (0, 0, 0, 0) (eq.5.10) 



Figure 30 and Figure 31 show body attitude and leg angle changes in time. Heun 
integration with a time step of 0.01 seconds was used for the results shown in Figure 26-3 1 . 
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Two Link Inverted Pendulum (Constant Length Massless 




Figure 26: Initial Orientation of the Two Link Inverted Pendulum (Lagrangian) 



i Two Link Inverted Pfendulum (Constant Loigth Massless Leg) LAGRANGIAN 




Figure 27: Two Link Inverted Pendulum Moving to the Upright Orientation by Control 

Torques(Lagrangian) 
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Two link Invmed Pendulum (Constant Length Massless Leg) LAGI^NGIAN 



Figure 28: Two Link Pendulum Recovering the Negative Orientations (Lagrangian) 




Figure 29: Two Link Inverted Pendulum After Steady State (Lagrangian) 
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Figure 30; Body Attitude Response of the Two Link Inverted Pendulum (Lagrangian) 




Figure 3 1 : Leg Angle Response of the Two Link Inverted Pendulum (Lagrangian) 
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3. Massless Leg and a Single Rigid Body with Newton-Euler Method 
Figure 32, Figure 33, Figure 34, and Figure 35 show the behavior of the two link 
inverted pendulum with control torques at the hip and at the ankle. Again, the z axis of the 
body is drawn to be able to observe the body attitude and the leg angle separately. The 
dynamic equations of the system are derived using the Newton-Euler Method. The mass of 

the rigid body,m , is 100 lb. The body rotary inertia, /, is 100 lb. ft.^. The length of the leg, 
r, is 3 ft. The length of rigid body, /, is 0.5 ft. The gravitational acceleration, g , is 32.2 ft. 
/ sec.^. The gain values for the control torques are 



= 25000 


(eq.5.11) 


K . = 2500 
0. 


(eq.5.12) 


= 40000 


(eq.5.13) 


K . =400 
02 


(eq.5.14) 


The initial state vector is 


X = (0.5,0, 1,0) 


(eq. 5.15) 


and the state vector in steady state is 


X = (0, 0, 0, 0) 


(eq.5.16) 



Figures 32 through 37 show body attitude and leg angle changes in time. Heun 
integration with a time step of 0.01 seconds was used for these results. 
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Figure 32: Initial Orientation of the Two Link Inverted Pendulum (Newton-Euler) 
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Two Link Inverted Pendulum (Constant Length Massless L^) NE\VTON-£ULE 




Figure 33: Two Link Pendulum Moving to the Upright Orientation (Newton-Euler) 
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Two Link Inverted Pendulum (Constant Length Massless Leg) NEWTON-EULEF 




Figure 34: Two Link Pendulum Recovering the Negative Orientations (Newton-Euler) 



;Two Link Inverted Pendulum (Constant Length Massless Leg) ^E^VTON-EULEP| 




Figure 35: Two Link Inverted Pendulum In Steady State (Newton-Euler) 



68 






Body Attitude (theta-2) NEWTON-EULER 




Figure 36: Body Attitude Time Response of the Two Link Inverted Pendulum (Newton- 

Euler) 



LegAncle (theta-1) NEWTON-EULER 
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Figure 37: Leg Angle Response of the Two Link Inverted Pendulum (Newton-Euler) 
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C. RESULTS OF THE DYNAMIC SIMULATION 



Euler integration was used for the single link inverted pendulum simulation. For the 
two link inverted pendulum simulations, Heun integration is chosen. Heun is a more 
accurate method than Euler, because it converges quadratically as the step size is decreased, 
while Euler integration converges only linearly. 

As seen in the previous section, the Newton-Euler and Lagrangian solutions of the 
two link inverted pendulum problem give identical results. However, the running times of 
these simulations are not the same. The time needed to complete the Newton-Euler version 
of the simulation is 20 percent longer than the Lagrangian version. This is an expected 
consequence of the matrix inversion which takes place in the Newton-Euler version. The 
Lagrangian version of the problem computes the accelerations without matrix inversion. 
However, it is hard to derive dynamic equations using Lagrangian methods for more 
complex models with higher degrees of freedom. Moreover, due to the complexity of such 
equations, it is suspected that Newton-Euler models for postural control may run faster for 
more complex systems. This will certainly be true if 0(n) methods are used [MCMI95]. 

D. KINEMATIC SIMULATION OF STEPPING DYNAMAN 

1. Stepping Forward Algorithm 

Figures 38 through 43 are six frames from the kinematic model simulation. These 
six frames show one step cycle of Dynaman during forward stepping. 
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Figure 38: Forward Stepping (1) 







Figure 39: Forward Stepping (2) 
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Figure 40: Forward Stepping (3) 




Figure 4 1 : Forward Stepping (4) 
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Figure 42; Forward Stepping (5) 




Figure 43; Forward Stepping (6) 
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2. Stepping Upward Algorithm 

Figure 44 through 48 are five frames from the kinematic model simulation. These 
five frames show one step cycle of the Dynaman while stepping upward. 




Figure 44: Upward Stepping (1) 
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Figure 45; Upward Stepping (2) 




Figure 46: Upward Stepping (3) 
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Figure 47: Upward Stepping (4) 




Figure 48: Upward Stepping (5) 
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VI. SUMMARY AND CONCLUSIONS 



A. SUMMARY 

The increasing demand for realistic 3D Virtual Environments requires more 
research on modeling human motion. There exist two main approaches to this problem: 
kinematic and dynamic modeling. A detailed review of previous work on kinematic 
modeling, dynamic modeling and control of graphical representation of human body 
motion is presented in this thesis. Kinematics uses only the geometric constraints of the 
model which results in less complicated models. However, for some situations, kinematic 
modeling may not be sufficient to simulate human motion realistically. Dynamics 
introduces additional properties of human limb segments to the simulation, such as mass 
and moment of inertia. These additional properties allow a more realistic simulation. The 
cost of this realism is an increase in computational complexity. The goal of the research of 
this thesis is to develop a realistic looking human model, while considering the complexity 
of the simulation according to the response time based on the computational power of 
current graphics hardware. 

To be able to create a more realistic, real time, and computationally efficient human 
model, a simple dynamic model which was proposed, but not fully developed in 
[MCGH79] was implemented. The model is a two link planar inverted pendulum which is 
made of a rigid body with mass and a supporting massless leg. Two different methods, 
generalized coordinates with Lagrangian, and the Newton-Euler, were used to derive the 
dynamic equations of the simulations. The implementation results verified models against 
each other and related models in the literature [GUBI74]. The problem of determining the 
gain parameters for limb segment control torques is also investigated. Since the degree of 
the characteristic equation is four, an experimental solution is presented, instead of an 
analytic one. Another result is the difference between the running times of the simulations. 
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The simulation which uses the Newton-Euler method takes 20% longer than the 
Lagrangian version, because of matrix inversions. It is also observed that the Lagrangian 
differential equations are simpler to use in deriving gain values for the system in 
comparison to the Newton-Euler version of the problem. However, the Lagrangian method 
would be hard to use in a more complex model with higher degrees of freedom. For such 
cases, the recursive Articulated Body algorithm [MCMI95] which has 0(n) complexity 
needs to be used. 

On the other hand, a detailed kinematic human model which contains 14 body parts 
was also developed. The simulation is based on joints angle instead of the Euler angles 

which used in previous work. IRIS Performer™ API, which interfaces to both IRIS GL™ 

and OpenGL. IRIS Performer™ and provides a hierarchy among Dynamic Coordinate 
Systems (DCS), was used to implement articulated chain structures easily. 

B. CONCLUSION AND FUTURE RESEARCH 

This thesis covers the basics for simplified dynamic human motion simulation. The 
model which is introduced in this thesis has only the torso with mass. An advanced version 
of this simulation may consider leg mass as well. The existing model is planar. Another step 
can be to investigate a 3D version of the model. The “Rigid-body” class could continue to 
be used for this purpose since provides support for simulation of body parts in 3D. 
Multilink more complex dynamic models also need to be investigated by considering 
different solution methods such as Newton-Euler, iterative use of inverse dynamics 
[KOOZ83], and the Articulated Body Algorithm [MCMI95]. Accuracy and time response 
of the system should be taken into account, while the complexity of the model is increased. 

Another issue is to integrate the existing kinematic and dynamic models. This 
integration may result a more realistic human model than the developed kinematic model. 
It will be computationally cheaper than a 14 link human dynamic model. 
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This thesis investigated posture control of a human model. Stepping is another 
important subject in human motion simulation. An efficient automatic stepping algorithm 
needs to be to investigated for a dynamically simulated human model. Active (dynamic) 
balancing should be considered while studying automatic stepping control. Advance 
features in further research in stepping is speed and direction control, and rough terrain 
locomotion for the walking human model. For direction and speed control 
implementations, user-computer interaction will become important. A speech recognition 
interface could be taken in the consideration as a realistic solution [DEVI96]. 

VRML is a fast growing 3D modeling language. It is possible to execute the needed 
computation by using Java Classes which are integrated to VRML nodes. Implementing the 
simulations by using VRML and Java Scripting would provide platform independency 
which would allow the simulations runable in other than graphic workstation 
environments. The author hopes that the work of this thesis will provide a foundation for 
continuing research in some of the above topics. 
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APPENDIX A: DYNAMIC SIMULATION SOFTWARE 



COMMON FILES IN ALL DYNAMIC SIMULATIONS 



*★***★*★**★**★***★**★★***★★*****★**★*★****★★*★*★*★******************* 
File : robot-kinematics . cl 

Author : Dr. R. McGhee 

: Naval Postgraduate School 
: Monterey, CA 93943 

Summary : This file contains various matrix and vector operation 

: functions. 

★*★***★*★*★★*★*★★★*★★★***★★**★*★*★★★★***★★★*★****★★******★*********** 



(defun transpose (matrix) ;A matrix is a list of row vectors. 

(cond ((null (cdr matrix)) (mapcar 'list (car matrix))) 

(t (mapcar 'cons (car matrix) (transpose (cdr matrix)))))) 

(defun dot-product (vector-1 vector-2) ;A vector is a list of nxomerical atoms 
(apply '+ (mapcar ’* vector-1 vector-2))) 

(defun vector-magnitude (vector) (sqrt (dot-product vector vector) ) ) 

(defun post-multiply (matrix vector) 

(cond ((null (rest matrix)) (list (dot-product (first matrix) vector))) 

(t (cons (dot-product (first matrix) vector) 

(post-multiply (rest matrix) vector) ) ) ) ) 

(defun pre-multiply (vector matrix) 

(post-multiply (transpose matrix) vector)) 

(defun matrix-multiply (A B) ;A and B are conformable matrices. 

(cond ( (null (cdr A) ) (list (pre-multiply (car A) B) ) ) 

(t (cons (pre-multiply (car A) B) (matrix-multiply (cdr A) B) ) ) ) ) 

(defun chain-multiply (L) ;L is a list of names of conformable matrices 

(cond ( (null (cddr L) ) (matrix-multiply (eval (car L) ) (eval (cadr L) ) ) ) 

(t (matrix-multiply (eval (car D) (chain-multiply (cdr L)))))) 

(defun cycle-left (matrix) (mapcar 'row-cycle-left matrix)) 

(defun row-cycle-left (row) (append (cdr row) (list (car row)))) 

(defun cycle-up (matrix) (append (cdr matrix) (list (car matrix)))) 

(defun unit-vector (one-column length) ;Column count starts at 1. 

(do ( (n length (1- n) ) 

(vector nil (cons (cond ( (= one-column n) 1) (t 0)) vector))) 

( (zerop n) vector))) 



(defun unit-matrix (size) 

(do ( (row- number size (1- row-number) ) 
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(I nil (cons (unit-vector row-number size) I))) 

( (zerop row- number) I) ) ) 

(defun concat-matrix (A B) ;A and B are matrices with equal number of rows, 
(cond ( (null A) B) 

(t (cons (append (car A) (car B) ) (concat-matrix (cdr A) 

(cdr B) ) ) ) ) ) 



(defun augment (matrix) 

(concat-matrix matrix (unit-matrix (length matrix)))) 

(defun normalize-row (row) ( scalar -multiply (/ 1.0 (car row)) row)) 



(defun scalar-mul tiply (scalar vector) 

(cond ((null vector) nil) 

(t (cons (* scalar (car vector)) 

( scalar-multiply scalar (cdr vector)))))) 



(defun solve-f irst-column (matrix) ;Reduces first column to (10 ... 0) . 

(do* ( (remaining-row-list matrix (rest remaining-row-list) ) 

(first-row (normalize-row (first matrix))) 

(answer (list first-row) 

(cons (vector-add (first remaining-row-list) 

(scalar-multiply (- (caar remaining-row-list) ) 
first-row) ) 



answer) ) ) 

((null (rest remaining-row-list)) (reverse answer)))) 



(defun vector-add (vector-1 vector-2) (mapcar ’+ vector-1 vector-2)) 



(defun vector-subtract (vector-1 vector-2) (mapcar '- vector-1 vector-2)) 

(defun first-square (matrix) ; Returns leftmost square matrix from argument, 
(do ( (size (length matrix) ) 

(remainder matrix (rest remainder) ) 

(answer nil (cons (firstn size (first remainder)) answer))) 

( (null remainder) (reverse answer) ) ) ) 



(defun firstn (n list) 

(cond ( (zerop n) nil) 

(t (cons (first list) (firstn (1- n) (rest list)))))) 

(defun max-car-f irstn (n list) 

(append (max-car-f irst (firstn n list)) (nthcdr n list))) 

(defun matrix- inverse (M) 

(do ( (Ml (max-car-f irst (augment M) ) 

(cond ((null Ml) nil) ;Abort for singular matrix. 

(t (max-car-f irstn n (cycle-left (cycle-up Ml)))))) 
(n (1- (length M) ) (1- n) ) ) 

((or (minusp n) (null Ml)) (cond ((null Ml) nil) (t (first-square Ml)))) 
(setq Ml (cond ((zerop (caar Ml)) nil) (t ( solve-f irst-column Ml)))))) 

(defun max-car- first (L) ;L is a list of lists. This function finds list with 

(cond ((null (cdr L) ) L) /largest car and moves it to head of list of lists. 
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(t (if (> (abs (caar L)) (abs (caar (max- car- first (cdr L))))) L 
(append (max-car-f irst (cdr L) ) (list (car L))))))) 

(defun dh-matrix (cosrotate sinrotate costwist sintwist length translate) 
(list (list cosrotate (- (* costwist sinrotate)) 

(* sintwist sinrotate) (* length cosrotate)) 

(list sinrotate (* costwist cosrotate) 

(- (* sintwist cosrotate)) (* length sinrotate)) 

(list 0. sintwist costwist translate) (list 0. 0. 0. 1.))) 

(defun homogeneous -transform (azimuth elevation roll x y z) 

(let ( (spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation)) 
(cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll))) 

(list (list (* cpsi cth) (- (* cpsi sth sphi) spsi cphi)) 

(+ (* cpsi sth cphi) spsi sphi)) x) 

(list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi)) 
(- (* spsi sth cphi) (* cpsi sphi)) y) 

(list (“ sth) cth sphi) (* cth cphi) z) 

(list 0. 0. 0. 1. ) ) ) ) 



(defun inverse-H (H) ;H is a 4x4 homogeneous transformation matrix, 

(let* ( (minus-P (list (- (fourth (first H) ) ) 

(- (fourth (second H) ) ) 

(- (fourth (third H) ) ) ) ) 

(inverse-R (transpose (first-square (reverse (rest (reverse H) ) ) ) ) ) 
(inverse-P (post-multiply inverse-R minus-P))) 

(append (concat-matrix inverse-R (transpose (list inverse-P))) 

(list (list 0001))))) 



(defun rotation-matrix (azimuth elevation roll) 

(let ((spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation)) 
(cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll))) 

(list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cphi)) 

(+ (* cpsi sth cphi) (* spsi sphi))) 

(list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi)) 
(- (* spsi sth cphi) (* cpsi sphi))) 

(list (- sth) (* cth sphi) (* cth cphi))))) 



(defun body-rate- to-euler-rate-matrix (azimuth elevation 
(let ((sth (sin elevation)) (cth (cos elevation)) (tth 
(sphi (sin roll)) (cphi (cos roll))) 

(list (list 1 (* tth sphi) (* tth cphi)) 

(list 0 cphi (- sphi)) 

(list 0 (/ sphi cth) (/ cphi cth))))) 



roll) 

(tan elevation) ) 
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/ / 

;; File : harmonic-equation . cl 

;; Author : Dr. R. McGhee 

; ; : Naval Postgraduate School 

; ; : Monterey, CA 93943 

; ; Summary : This file contains functions to create a window and execute 

; ; : drawing operations on the window. 

/ / 

(require :xcw) 

(cw: ini tialize-common-windows ) 

(defun make-window () 

(cw: make-window-stream : borders 5 

:left 10 
ibottom 280 
:width 600 
: height 600 

: title "Harmonic Equation" 

:activate-p t) ) ;Make window visible. 

(defun scale-point-coordinates (x-y-list enlargement- factor ) 

(let ( (x (first x-y-list)) (y (second x-y-list))) 

(list (+ 50 (round (* enlargement- factor x) ) ) 

(+ 225 (round (* enlargement- factor y) ) ) ) ) ) 

(defun draw-coordinate-axes (window) 

(cw: draw- line window (cw: make-position :x 20 :y 225) 

(cw:make-position :x 570 :y 225) 

: brush-width 2) 

(cw: draw- line window (cw: make-position :x 50 :y 20) 

(cw.make-position :x 50 :y 560) 

: brush-width 2)) 

(defun draw- line- in-window (window enlargement-factor start end) 

(let ((scaled-start (scale-point-coordinates start enlargement-factor)) 
(scaled-end (scale-point-coordinates end enlargement- factor) ) ) 

( cw: draw-line window 

(cw:make-position :x (first scaled-start) :y (second scaled-start) ) 
(cw:make-position :x (first scaled-end) :y (second scaled-end))))) 
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SINGLE LINK INVERTED PENDULUM SIMULATION WITH NEWTON-EULER 






File 

Author 



Modified by 
Summary 



euler-angle-rigid-body . cl 
Dr. R. McGhee 
Naval Postgraduate School 
Monterey, CA 93943 
Mehmet Bediz 

This file contains rigid body class which is implemented 
by Dr. R. McGhee. The function "test-rigid-body- forces-and- 
torques-three-with-M" and the other functions called by this 
function are implemented by Mehmet Bediz. 



★ ★★★★★★★★★★★★★★★*★*★★★★★*★*★★*★★★★★★★★★*★**★★★•*•**★•*•■***•*•■*•■*•**•*•**■*•*****.■*• 



;The vector (xe ye ze phi theta psi) 



(defclass rigid-body 

0 

{ (posture 

rinitform ’(000000) 

: initarg : posture 
: accessor posture) 

(posture-rate ; The vector (xe-dot ye-dot ze-dot phi-dot theta-dot psi-dot) . 

: initarg : posture-rate 
: accessor posture-rate) 

(velocity ;The six-vector (u v w p q r) in body coordinates, 

linitform '(000 000) 

: initarg ivelocity 
•.accessor velocity) 

(velocity-growth-rate ;The vector (u-dot v-dot w-dot p-dot q-dot r-dot) . 

: accessor velocity-growth-rate) 

( f orces-and-torques ;The vector (Fx Fy Fz L M N) in body coordinates. 

; linitform (list 0 0 (- *gravity*) 000) 
rinitform (list 000000) 

: accessor f orces-and- torques ) 

(moments-of-inertia ;The vector (Ix ly Iz) in principal axis coordinates, 
linitform '(1 900 1) 

: initarg :moments-of-inertia 
: accessor moments-of-inertia) 

(mass 

linitform 100 
: initarg imass 
; accessor mass) 

(node-list ; (x y z 1) 
linitform ' ( (0 0 0 1) 



in body coord for each node. Starts with (0 0 
(-0.5 -0.5 0.5 1) (0.5 -0.5 0.5 1)(0.5 -0.5 -0. 
(-0.5 -0.5 -0.5 l)(-0.5 0.5 0.5 1)(0.5 0.5 0.5 1) 
(0.5 0.5 -0.5 l)(-0.5 0.5 -0.5 1) (0 001) 

(0 0 3 D) 



1 ) . 
1 ) 



: initarg mode-list 
laccessor node-list) 

(polygon-list 

linitform '((1 5 8 4) (5 6 7 8) (6 2 3 7) (2 1 4 3) (9 10)) 

1 initarg : polygon-list 
I accessor polygon-list) 

(transformed-node-list ; (x y z 1) in earth coord for each node in node-list, 
linitform ' ( (0 0 0 1) (-2 -2 0 1) (2 -2 0 1) (2 -2 -30 1) (-2 -2 -30 1) 

(-2 2 0 1) (2 2 0 1) (2 2 -30 1) (-2 2 -30 1)) 
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: accessor transf ormed-node-list ) 

(H~matrix 

linitform (unit-matrix 4) 

: accessor H-matrix) 

( time- stamp 

: accessor time-stamp))) 

(defmethod initialize ( (body rigid-body) ) 

(setf ( transf ormed-node-list body) (node-list body)) 

(setf (velocity-growth-rate body) (update-velocity-growth-rate body) ) 

(setf (posture-rate body) (earth-velocity body) ) 

(setf (time-stamp body) (get-internal-real-time)))) 

(defmethod set-transf ormed-node-list-z ( (body rigid-body) z) 

(setf ( third ( fourth ( transf ormed-node-list body) ) ) (- z) ) 

(setf (third(fifth ( transf ormed-node-list body))) (- z) ) 

(setf ( third (eighth ( transf ormed-node-list body))) (- z) ) 

(setf (third(ninth ( transf ormed-node-list body))) (- z) ) 

( transf ormed-node-list body) ) ) 

(defun transform (obj) 

(list 0 (first obj) (second obj) (third obj))) 

(defun reverse- transform (obj) 

(list (second obj) (third obj) (fourth obj) 1)) 

(defmethod move ( (body rigid-body) azimuth elevation roll x y z) 

(setf (posture body) (list x y z roll elevation azimuth)) 

(setf (H-matrix body) 

(homogeneous-transform azimuth elevation roll x y z) ) 

( transf orm-node- list body)) 

(defmethod get-delta-t ((body rigid-body)) 0.02) 

; (let* ( (new- time (get-internal-real-time) ) 

; (delta-t (/ (- new-time (time-stamp body)) 1000))) 

; (setf (time- stamp body) new- time) 

; delta-t) ) 

(defmethod update-rigid-body ((body rigid-body)) ;Euler integration. 

(let* ((delta-t (get-delta-t body))) 

(update-posture body delta-t) ; EULER 
(update-velocity body delta-t) ; EULER 

(setf (H-matrix body) (homogeneous- transform (sixth (posture body) ) 

(fifth (posture body)) (fourth (posture body)) (first (posture body)) 
(second (posture body) ) (third (posture body) ) ) ) 

( transf orm-node- list body) 

(update-velocity-growth-rate body) ) ) 

(defmethod update-velocity-growth-rate ( (body rigid-body) ) 

(setf (velocity-growth-rate body) ;Assumes principal axis coordinates with 
(multiple-value-bind ; origin at center of gravity of body. 

(Fx Fy Fz L M N u V w p q r Ix ly Iz) ; Declares local variables, 
(values-list jValues assigned. 

(append 

( forces-and-torques body) (velocity body) (moments-of-inertia body))) 
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{list {+ 


{* 


V r) 


{* - 


1 w q) {/ Fx {mass body)) 




{* 


* gravity* 


{first 


{third {H-matrix body))))) 


{ + 


{* 


w p) 


{* - 


1 u r) {/ Fy {mass body)) 




{* 


* gravity* 


{second 


{third {H-matrix body))))) 


{ + 


{* 


u q) 


{* - 


1 V p) {/ Fz {mass body)) 




{* 


* gravity* 


{third 


(third (H-matrix body))))) 


{/ 


{ + 


{* {- 


ly 


Iz) q r) 


L) Ix) 


{/ 


{ + 


{* {- 


I2 


Ix) r p) 


M) ly) 


{/ 


{ + 


{* {- 


Ix 


ly) P q) 


N) Iz))))) ;Value returned 



(defmethod update-velocity {{body rigid-body) delta-t) ;Euler integration. 

{setf {velocity body) 

{vector-add {velocity body) 

{scalar-multiply delta-t {velocity-growth-rate body))))) 

{defmethod update-posture {{body rigid-body) delta-t) ;Euler integration. 

{setf {posture-rate body) {earth- velocity body) ) 

{setf {posture body) 

{vector-add {posture body) {scalar-multiply delta-t {posture-rate body))))) 

{defmethod transform-node- list { {body rigid-body) ) 

{setf { transformed-node-list body) 

{mapcar #' {lambda {node-location) 

{post-multiply {H-matrix body) node-location) ) 

{node-list body) ) ) ) 

{def constant *gravity* 32.2185) 

{defmethod earth-velocity { {body rigid-body) ) 

{let* {{linear-velocity {firstn 3 {velocity body))) 

{rotational-velocity {cdddr {velocity body) ) ) 

{posture {posture body) ) 

{R-matrix {rotation-matrix {sixth posture) {fifth posture) 

{fourth posture))) 

{linear-earth-velocity {post-multiply R-matrix linear-velocity) ) 
{T-matrix {body-rate-to-euler-rate-matrix {sixth posture) 

{fifth posture) {fourth posture))) 
{rotational-earth-velocity {post-multiply T-matrix 

rotational-velocity) ) ) 

{append linear-earth-velocity rotational-earth-velocity) ) ) 

{defun test-rigid-body {) 

{setf airplane-1 {make- instance 'rigid-body)) 

{initialize airplane-1) 

{setf camera-1 {make- instance ' strobe- camera ) ) 

{move camera- 1 {- {/pi 2)) 0 00300) 

{take-picture camera- 1 airplane-1) 

{dotimes {i 20 'done) {update-rigid-body airplane-1)) 

{take-picture camera- 1 airplane-1) ) 
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t t i i i 



Inverted Pendulum (fixed leg length) 
state vector x = (theta theta-dot) 
state vector u = (M) 

- control torque at the pivot (M) 

Newton-Euler Method 

3x3 matrix inversion to calculate forces and moments 
RIGID Body class to compute accelerations 
Heun Integration 



(defmethod f orces-earth-to-body (Fx-e Fy-e Fz-e theta) 

(let* ( (forces-earth (list Fx-e Fy-e Fz-e)) 

(R-matrix (rotation-matrix 0 (- theta) 0)) ;Since planar 
(forces-body (post-multiply R-matrix forces-earth) ) ) 

forces-body) ) 

(defmethod update- forces-and- torques- three-with-M ( (body rigid-body) M) 

(let* ( (equations-solution-vector ( solve-three-equation-sys tem-with-M 

body M) ) 

(Fx-earth (second equations-solution-vector) ) 

(Fz-earth (third equations-solution-vector)) 

(F-body ( f orces-earth-to-body Fx-earth 0 Fz-earth 

(fifth (posture body))))) 

(setf forces-and-torques (list (first F-body) 

0 

(third F-body) 

0 

(* (first equations-solution-vector) 
(second (moments-of-inertia body) ) ) 

0 ) ) ) ) 



(defun solve-three-equation-system-with-M (body M) 
(setf m (mass body) ) 

(setf 1 3) 

(setf I (second (moments-of-inertia body) ) ) 

(setf theta (fifth (posture body))) 

(setf theta-dot (fifth (velocity body))) 

(setf g *gravity*) 



(post-multiply (matrix- inverse (list (list I (*-11 (cos theta)) 

( * 1 (sin theta) ) ) 

(list (* m 1 (cos theta)) 1 0 ) 

(list (* m 1 (sin theta) ) 0 -1 ) ) ) 

(list M 

(* (sqr theta-dot) m 1 (sin theta)) 

(- (* m g) (* (sqr theta-dot) m 1 (cos theta)))))) 
(defun compute-M (body K- theta K-theta-dot) 

(setf theta (fifth (posture body))) 

(setf theta-dot (fifth (velocity body))) 
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(+ (* -1 K-theta theta) {* -1 K-theta-dot theta-dot))) 



{defun test-rigid-body-forces-and-torques-three-with-M (K-theta K-theta-dot) 
(setf inverted-pendulum (make-instance 'rigid-body)) 

(initialize inverted-pendulum) 

(setf camera-1 (make- instance ' strobe- camera ) ) 

(move camera-1 (deg-to-rad (- 90) ) (deg-to-rad 0) (deg-to-rad 0) 0 10 0) 

(move inverted-pendulum 0-10000) 

;( take-picture camera-1 inverted-pendulum) 

(setf graph- 1-window (make-window-graph- 1) ) 

(draw-coordinate-axes graph- 1 -window) 

(setf old-theta 0.5) 

(do { (i 0 (+ i 1) ) ) 

( ( > i 1000 ) ' end) 

(setf { forces-and- torques inverted-pendulum) 

(update-f or ces -and- torques -three-with-M inverted-pendulum 
(compute-M inverted-pendulum K-theta K-theta-dot) ) ) 
(update- rigid-body inverted-pendulum) 

(cwiclear (camera-window camera-1)) 

(take-picture camera-1 inverted-pendulum) 

(draw-line-in-window graph- 1 -window 80 

(list 

(* (get-delta- t inverted-pendulum) (- i 1) ) 
(* -1 old-theta) ) 

(list 

(* (get-delta-t inverted-pendulum) i) 

(* -1 (fifth (posture inverted-pendulum))))) 

(setf old-theta (fifth (posture inverted-pendulum))))) 

(defun sqr(x) 

(* X X)) 

(defun make-window-graph-1 () 

( cw: make- window- stream : borders 5 

:left 0 
rbottom 550 
:width 450 
rheight 450 

: title "Body Attitude (theta) NEWTON-EULER" 
:activate-p t) ) ;Make window visible. 
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File : load-euler- files . cl 

Author : Mehinet Bediz 

: Naval Postgraduate School 
: Monterey, CA 93943 

Stunmary : This file contains functions to load files and to test 

: the simulations. 



; Load files 

77//#7i//////////////////7////////////////////// 

(load "harmonic-equation. cl " ) 

(load "robot-kinematics . cl” ) 

(load "euler-angle-rigid-body . cl " ) 

(load "strobe-camera. cl" ) 



Tests 



/////////////////////////////////////////////////////////////////////// 



(defun graph_l () 

(solve-robot-attitude 0.01 1.0 10000 2000 109660 20000)) 
(defun graph_2 () 

(solve-robot-altitude-z_zO 0.01 1.0 10000 2000 109660 20000)) 
(defun graph_3 () 

(solve-robot-altitude-z 0.01 1.0 10000 2000 109660 20000)) 

(defun draw-converted-pendulum-test-real-parameters () 

(draw-converted-pendulum 0.01 2.0 10000 2000 109660 20000)) 

(defun draw-converted-pendulum-test-zero-gain () 

(draw-converted-pendulum 0.01 2.0 0 0 0 0)) 
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File : strobe-camera . cl 

Author : Dr. R. McGhee 

: Naval Postgraduate School 
: Monterey, CA 93943 

Summary : This file contains strobe-camera class definitions. 

**-k*-k**-k*-k-k-k-k-k*-k**-k-k-k***-k-k*-k-k*'k**if-kieir'kiritieir*ir**-k-*cicir*ir*leie*itir-kir-k-k*-kkir*ieir 



(require :xcw) 

(cw: initialize-common-windows) 

(defclass strobe-camera (rigid-body) 

( ( focal-length 

: accessor focal-length 
linitform 6) 

( camera-window 
: accessor camera-window 

: initf orm (cw: make- window- stream iborders 5 

:left 500 
rbottom 500 
: width 650 
iheight 650 

: title " strobe- camera- image" 

:activate-p t) ) 

(H-matrix 

:initform (homogeneous- transform .3 -.3 0 -300 -90 -90)) 

( inverse-H-matrix 
•.accessor inverse-H-matrix 

linitform (inverse-H (homogeneous- transform .3 -.3 0 -300 -90 -90))) 
(enlargement- factor 
: accessor enlargement- factor 
: initform 30) ) ) 

(defmethod move ( (camera strobe -camera) azimuth elevation roll x y z) 

(setf (H-matrix camera) (homogeneous -transform azimuth elevation roll x y z)) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera)))) 

(defmethod take-picture ( (camera strobe-camera) (body rigid-body) ) 

(let ( {camera-space-node-list (mapcar #' (lambda (node-location) 

(post-multiply (inverse-H-matrix camera) node-location)) 

( transf ormed-node-list body)))) 

(dolist (polygon (polygon-list body) ) 

(clip-and-draw-polygon camera polygon camera-space-node-list)))) 

(defmethod clip-and-draw-polygon 

( (camera strobe- camera) polygon node-coord-list) 

(do* ((initial-point (nth (first polygon) node-coord-list)) 

(from-point initial-point to-point) 

(remaining-nodes (rest polygon) (rest remaining-nodes) ) 

(to-point (nth (first remaining-nodes) node-coord-list) 

(if (not (null (first remaining-nodes))) 

(nth (first remaining-nodes) node-coord-list)))) 

( (null to-point) 

(draw-clipped-projection camera from-point initial-point) ) 
(draw-clipped-projection camera from-point to-point) ) ) 
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(defmethod draw-clipped-projection ( (camera strobe- camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera)) 

(<= (first to-point) (focal-length camera))) nil) 

( (<= (first from-point) (focal-length camera)) 

(draw- line- in- camera-window camera 

(perspective-transform camera (from-clip camera from-point to-point)) 
(perspective- transform camera to-point) ) ) 

( (<= (first to-point) (focal-length camera)) 

(draw- line- in- camera-window camera 

(perspective-transform camera from-point) 

(perspective- transform camera (to-clip camera from-point to-point) ) ) ) 
(t (draw- line-in-camera-window camera 

(perspective-transf orm camera from-point) 

(perspective- transform camera to-point) ) ) ) ) 

(defmethod from-clip ( (camera strobe-camera) from-point to-point) 

(let ((scale-factor (/ (- (focal-length camera) (first from-point)) 

(- (first to-point) (first from-point))))) 

(list (+ (first from-point) 

(* scale-factor (- (first to-point) (first from-point)))) 

(+ (second from-point) 

(* scale-factor (- (second to-point) (second from-point)))) 

(+ (third from-point) 

{* scale-factor (- (third to-point) (third from-point)))) 1))) 

(defmethod to-clip ( (camera strobe- camera) from-point to-point) 

(from-clip camera to-point from-point)) 



(defmethod draw- line -in- camera -window ( (camera strobe- camera) start end) 

(cw: draw- line (camera-window camera) 

(cwrmake-position :x (+ 150 (first start)) :y (+ 150 (second 

start) ) ) 



end) ) ) 



(cwimake-position :x (+ 150 (first end)) :y (+ 150 (second 



: brush-width 0)) 



(defmethod perspective- transform ( (camera strobe-camera) point-in-camera-space) 
(let* ( (enlargement- factor (enlargement- factor camera)) 

(focal-length (focal-length camera)) 

(x (first point-in-camera-space)) ;x axis is along optical axis 
(y (second point-in-camera-space) ) ;y is out right side of camera 
(z (third point- in-camera-space) ) ) ;z is out bottom of camera 
(list (h- (round (* enlargement- factor (/ (* focal-length y) x) ) ) 

150) ; to right in camera window 

(+ 150 (round (* enlargement- factor (/ (* focal-length (- z) ) x) ) 

))))) ;up in camera window 

(defun test-camera (z theta) ; Produces top view of default rigid-body. 

(setf robot-leg (make- instance 'rigid-body)) 

(initialize robot-leg) 

(set- transf ormed-node-list-z robot-leg z) 

(set-transf ormed-node-list-theta robot-leg theta) 

(setf camera-1 (make- instance ' strobe- camera ) ) 

(move camera-1 (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) -30 0 0) 
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(take-picture camera-1 robot-leg)) 



(defun deg-to-rad (angle) (* .01745329251994329 angle)) 
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TWO LINK INVERTED PENDULUM SIMULATION WITH LAGRANGIAN 






File 

Author 



Modified by 
Suinmary 



euler-angle-rigid-body . cl 
Dr . R . McGhee 
Naval Postgraduate School 
Monterey, CA 93943 
Mehmet Bediz 

This file contains rigid body class which is implemented 
by Dr. R. McGhee. The function "draw- inverted-pendulum" 
and the other functions called by this 
function are implemented by Mehmet Bediz. 






;The vector (xe ye ze phi theta psi) 



(defclass rigid-body 

0 

( (posture 

rinitform '(000000) 
linitarg rposture 
: accessor posture) 

(posture-rate ;The vector (xe-dot ye-dot ze-dot phi-dot theta-dot psi-dot) . 



;The six-vector (u v w p q r) 
0 0 0 ) 



in body coordinates . 



linitarg :posture-rate 
laccessor posture-rate) 

(velocity 
: initf orm '(000 
linitarg ivelocity 
raccessor velocity) 

(velocity-growth-rate ;The vector (u-dot v-dot w-dot p-dot q-dot r-dot) . 

: accessor velocity-growth-rate) 

( f orces-and-torques ;The vector (Fx Fy Fz L M N) in body coordinates, 

rinitform (list 000000) 

: accessor f orces-and-torques ) 

(moments-of -inertia ;The vector (Ix ly Iz) in principal axis coordinates, 
rinitform ' (1 100 1) 
rinitarg rmoments-of-inertia 
r accessor moments-of-inertia) 

(mass 

rinitform 100 
rinitarg rmass 
raccessor mass) 

(node-list ; (x y z 1) in body coord for each node. Starts with (0001). 
rinitform ’((000 1) (-0.5 -0.5 0.5 1) (0.5 -0.5 0.5 1)(0.5 -0.5 -0.5 1) 

(-0.5 -0.5 -0.5 l)(-0.5 0.5 0.5 1) (0.5 0.5 0.5 1) 

(0.5 0.5 -0.5 l)(-0.5 0.5 -0.5 1)(0 0 0 1) (0 0 3 D) 



rinitarg mode-list 
raccessor node-list) 

(polygon-list 

rinitform '( (1 5 8 4) (5 6 7 8) (6 2 3 7) (2 1 4 3 ) (9 10)) 
rinitarg rpolygon-list 
raccessor polygon-list) 

( transformed-node-list ; (x y z 1) in earth coord for each node in node-list, 
rinitform '((0001) (-2 -2 0 1) (2 -2 0 1) (2 -2 -30 1) (-2 -2 -30 1) 

(-2 2 0 1) (2 2 0 1) (2 2 -30 1) (-2 2 -30 1)) 

raccessor transformed-node-list) 

(H-matrix 



94 



: ini t form (unit-matrix 4) 

: accessor H-matrix) 

( time- stamp 

: accessor time-stamp) ) ) 

(defmethod initialize ((body rigid-body)) 

(setf ( transformed-node-list body) (node-list body)) 

(setf (velocity-growth-rate body) (update-velocity-growth-rate body) ) 

(setf (posture-rate body) (earth-velocity body) ) 

(setf (time-stamp body) (get-internal-real-time)))) 

(defmethod set-transf ormed-node-list-z ( (body rigid-body) z) 

(setf ( third ( fourth (transformed-node-list body) ) ) (- z) ) 

(setf (third(fifth (transformed-node-list body))) (- 2 )) 

(setf (third (eighth (transformed-node-list body))) (- z) ) 

(setf (third(ninth (transformed-node-list body))) (- z) ) 
(transformed-node-list body))) 

(defun transform (obj) 

(list 0 (first obj) (second obj) (third obj))) 

(defun reverse- transform (obj) 

(list (second obj) (third obj) (fourth obj) 1)) 

(defmethod move ((body rigid-body) azimuth elevation roll x y z) 

(setf (posture body) (list x y z roll elevation azimuth) ) 

(setf (H-matrix body) 

(homogeneous- transform azimuth elevation roll x y z)) 

(transform-node-list body)) 

(defmethod get-delta-t ((body rigid-body)) 0.005) 

(defmethod f orces-earth-to-body (Fx-e Fy-e Fz-e theta) 

(let* ( ( f orces-earth (list Fx-e Fy-e Fz-e)) 

(R-matrix (rotation-matrix 0 (- theta) 0) ) 

(forces-body (post-multiply R-matrix f orces-earth) ) ) 

forces-body) ) 

(defmethod update-rigid-body ((body rigid-body)) ;Euler integration. 

(let* ( (delta-t (get-delta-t body))) 

(update-posture body delta-t) ; EULER 
(update-velocity body delta-t) ; EULER 

(setf (H-matrix body) (homogeneous- transform (sixth (posture body) ) 

(fifth (posture body)) (fourth (posture body)) (first (posture body)) 
(second (posture body)) (third (posture body)))) 

(transform-node-list body) 

(update-velocity-growth-rate body) ) ) 

(defmethod update-velocity-growth-rate ( (body rigid-body) ) 

(setf (velocity-growth-rate body) ;Assumes principal axis coordinates with 
(multiple-value-bind ; origin at center of gravity of body. 

(Fx Fy Fz L M N u v w p q r Ix ly Iz) /Declares local variables. 
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/■Values assigned. 



(values-lis t 
(append 

(forces-and- torques body) (velocity body) (moments -of- inertia body) ) ) 



(list (+ 


{* 


V r) 


(* - 


1 w q) ( / Fx (mass body)) 




(* 


*gravity* 


(first 


(third (H-matrix body))))) 


( + 


(* 


w p) 


(* - 


1 u r) (/ Fy (mass body)) 




(* 


* gravity* 


(second 


(third (H-matrix body) ) ) ) ) 


( + 


(^ 


u q) 


(* - 


1 V p) (/ Fz (mass body)) 




(* 


* gravity* 


(third 


(third (H-matrix body) ) ) ) ) 


(/ 


( + 


(* (- 


ly 


Iz) q r) 


L) Ix) 


(/ 


( + 


(* (- 


Iz 


Ix) r p) 


M) ly) 


(/ 


( + 


(* (- 


Ix 


ly) P q) 


N) Iz) ) ) ) ) ; Value returned 



(defmethod update-velocity ((body rigid-body) delta-t) ;Euler integration. 

(setf (velocity body) 

(vector-add (velocity body) 

(scalar-multiply delta-t (velocity-growth-rate body) ) ) ) ) 

(defmethod update-posture ( (body rigid-body) delta-t) ; Euler integration. 

(setf (posture-rate body) (earth-velocity body)) 

(setf (posture body) 

(vector-add (posture body) (scalar-multiply delta-t (posture-rate body) ) ) ) ) 

(defmethod transform-node-list ((body rigid-body)) 

(setf ( transformed-node-list body) 

(mapcar #’ (lambda (node-location) 

(post-multiply (H-matrix body) node-location) ) 

(node-list body) ) ) ) 

(defconstant ^gravity* 32.2185) 

(defmethod earth-velocity ( (body rigid-body) ) 

(let* ((linear-velocity (firstn 3 (velocity body))) 

(rotational-velocity (cdddr (velocity body) ) ) 

(posture (posture body) ) 

(R-matrix (rotation-matrix (sixth posture) (fifth posture) 

(fourth posture) ) ) 

(linear-earth-velocity (post-multiply R-matrix linear-velocity)) 
(T-matrix (body-rate- to-euler-rate-matrix (sixth posture) 

(fifth posture) (fourth posture))) 
(rotational-earth-velocity (post-multiply T-matrix 

rotational-velocity) ) ) 

(append linear-earth-velocity rotational-earth-velocity) ) ) 

(defun test-rigid-body () 

(setf airplane-1 (make- instance 'rigid-body)) 

(initialize airplane-1) 

(setf camera-1 (make-instance ' strobe- camera ) ) 

(move camera-1 (- (/ pi 2)) 0 0 0 30 0 ) 

(take-picture camera-1 airplane-1) 

(dotimes (i 20 'done) (update-rigid-body airplane-1)) 

(take-picture camera-1 airplane-1)) 
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- Two Link Inverted Penduliim 

- Generalized Coordinates with Lagrangian Method 

- Massless leg and a single rigid body (fixed leg length) 

- State vector x = (theta-1 theta-l-dot theta-2 theta-2-dot) 

- State vector u = (M-1 M-2) 

- control torque at the ankle (M-1) 

- control torque at the hip (M-2) 

- Heun Integration 

- By Mehmet Bediz 



(setf 1 0.5) 

(setf r 3) 

(setf time-step 0.01) 

(defun euler-step-constant-leg (state-vector M-1 M-2) 

(mapcar '+ state-vector (scalar-multiply time-step 

(derivative -state -vector 
state-vector M-1 M-2)))) 

(defun heun-step-constant-leg (x K-theta-1 K- theta-l-dot 

K-theta-2 K-theta-2-dot) 

(setf M-1 (compute-M-1 x K-theta-1 K- theta-l-dot 

K-theta-2 K- theta-2-dot) ) 
(setf M-2 (compute-M-2 x K-theta-1 K- theta-l-dot 

K-theta-2 K-theta-2 -dot ) ) 
(mapcar '+ x (scalar-multiply (* time-step .5) 

(vector-add (derivative-state-vector x M-1 M-2) 

(derivative -state- vector 

(euler-step-constant-leg x M-1 M-2) M-1 M-2))))) 

(defun derivative-state-vector (state-vector M-1 M-2) 

(list (second state-vector) 

(compute- theta-l-double-dot state-vector M-1 M-2) 
(fourth state-vector) 

(compute- theta-2 -double-dot state-vector M-1 M-2))) 



(defun compute-theta-l-double-dot (state-vector M-1 M-2) ; Eq 3.51 



(setf 


m 100) 






( setf 


1 0.5) 






( setf 


r 3) 






(setf 


I 100) 






( setf 


theta-1 


(first 


state-vector) ) 


( setf 


theta-l-dot 


(second 


state-vector) ) 


( setf 


theta-2 


(third 


state-vector) ) 


( setf 


theta-2-dot 


( fourth 


state-vector) ) 


( setf 


g 32.2) 






( / 


(+ (* 1 1 m m r 


g theta- 


2) 




(* -1 m g r 


(+ I (♦ m 1 1) ) theta-1 




(* -1 (+ I ( 


’ml 1) ) 


M-1) 




(* (+ (* 1 m 


r) I (* 


mil)) M-2) 



) 

(* -1 I m r r) ) ) 
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(defun compute-theta-2-double-dot (state-vector M-1 M-2) ; Eq 3.54 



(setf 

(setf 

(setf 

(setf 

(setf 


m 100) 

1 0.5) 

r 3) 

I 100) 

theta-1 


( first 


state-vector) ) 


(setf 


theta-l-dot 


(second 


state-vector) ) 


(setf 


theta-2 


( third 


state-vector) ) 


(setf 


theta-2-dot 


( fourth 


state-vector) ) 


(setf 


g 32.2) 







( / (+ (* m g 1 r theta-2) 

(* -1 m g 1 r theta-1) 
(* -1 1 M-1) 

(* (+ r 1) M-2) 

) 

(* r I) 

) 

) 



(defun compute-M-1 (state-vector K-theta-1 K-theta-l-dot 

K-theta-2 K- theta-2-dot) 

(setf theta-1 (first state-vector)) 

(setf theta-l-dot (second state-vector) ) 

(+ (* -1 K-theta-1 theta-1) (* -1 K-theta-l-dot theta-l-dot))) 

(defun compute-M-2 (state-vector K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot) 

(setf theta-2 (third state-vector) ) 

(setf theta-2-dot (fourth state-vector)) 

(+ (* -1 K-theta-2 theta-2) (* -1 K- theta-2-dot theta-2-dot) ) ) 

(defun compute-y (state-vector) 



( setf 


theta- 1 


(first 


state-vector) ) 


(setf 


theta-l-dot 


(second 


state-vector) ) 


(setf 


theta-2 


(third 


state-vector) ) 


( setf 


theta-2-dot 


( fourth 


state-vector) ) 


( setf 


1 0.5) 






(setf 


r 3) 






(+ (* 


r (sin theta- 


1) ) 





(* 1 (sin theta-2) ) ) ) 



(defun compute -2 (state-vector) 



(setf 


theta-1 


(first 


state-vector) ) 


( setf 


theta-l-dot 


(second 


state-vector) ) 


( setf 


theta-2 


(third 


state-vector) ) 


(setf 


theta-2-dot 


( fourth 


state-vector) ) 


(setf 


1 0.5) 






( setf 


r 3) 






(+ (* 


r (cos theta- 


1) ) 





(* 1 (cos theta-2)))) 



(defun draw- inverted-pendulum (halt-time K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot ) 
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(setf robot-body (make- instance 'rigid-body)) 
(initialize robot-body) 

(setf robot-leg (make- ins tance 'rigid-body)) 
(initialize robot-leg) 



(setf camera-1 (make- ins tance 'strobe-camera)) 

(move camera-1 (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) -10 0 0) 

(setf graph- 1 -window ( make- window- graph- 1 ) ) 

( draw-coordinate-axes graph- 1-window) 

(setf graph- 2 -window (make-window-graph-2)) 

(draw- coordinate-axes graph- 2 -window) 

(do ( (x '(0.5 0 1.0 0) (heun-s tep-constant-leg x K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot) ) 

(old-x ' (0.5 0 1.0 0) X) 

( time 0 (+ time 1 ) ) ) 

( (> time halt-time) 'done) 



(move robot-body (deg-to-rad 0) (deg-to-rad 0) (third x) 

0 (compute-y x) (* -l(compute-z x) ) ) 



(setf ( transformed-node-list robot-leg) (list (list 0001) 

(list 0 (* r (sin (first x) ) ) 
(* -1 r (cos (first x) ) ) 1) 
(list 0001))) 

(setf (polygon-list robot-leg) (list (list 2 1))) 



(cw:clear (camera-window camera-1)) 
(take-picture camera-1 robot-body) 
(take-picture camera-1 robot-leg) 

(draw- line- in-window graph-1 -window 80 (list 

(list 

(draw- line- in-window graph- 2 -window 80 (list 

( list 



(* time-step (- time 1) ) 
( first old-x) ) 

(* time-step time) 

(first x) ) ) 

(* time-step (- time 1)) 
(third old-x)) 

(* time-step time) 

(third X) ) ) ) ) 



(defun make- window-graph- 1 () 

(cw:make-window- stream : borders 5 



:left 0 
: bottom 50 
rwidth 450 
iheight 450 

: title "Leg Angle (theta-1) LAGRANGIAN" 
:activate-p t) ) ;Make window visible. 



(defun make-window-graph- 2 () 

(cw: make- window- stream : borders 5 

:left 0 
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bottom 550 
width 450 



height 450 

title "Body Attitude 


(theta- 2) LAGRANGIAN 



activate-p t) ) ;Make window visible 
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★ ★ * 






; File 
; Author 



; Suiranary 



load-euler-f lies . cl 
Mehmet Bediz 

Naval Postgraduate School 
Monterey, CA 93943 

This file contains functions to load files. 









Load files 



{load "harmonic-equation . cl '' ) 

(load "robot-kinematics . cl" ) 

(load "euler-angle-rigid-body . cl " ) 
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File 

Author 



Suimiary 

********** 



: strobe-camera . cl 
: Dr. R. McGhee 
: Naval Postgraduate School 
: Monterey, CA 93943 

: This file contains strobe-camera class definitions. 
********************************************************** 



(require :xcw) 

(cw: initialize-common-windows) 

(defclass strobe-camera (rigid-body) 

( ( focal-length 

laccessor focal-length 
:initform 6) 

( camera-window 
.•accessor camera-window 

rinitform (cw: make-window- stream iborders 5 

:left 470 
:bottom 500 
:width 650 
iheight 650 

: title "Two Link Inverted Pendulum 

(Constant Length Massless Leg) 
LAGRANGIAN" 

:activate-p t) ) 

(H-matrix 

:initform (homogeneous- transform .3 -.3 0 -300 -90 -90)) 

( inverse-H-matrix 
: accessor inverse-H-matrix 

linitform (inverse-H (homogeneous -transform .3 -.3 0 -300 -90 -90))) 
(enlargement- factor 
: accessor enlargement-f actor 
: initform 30) ) ) 

(defmethod move ( (camera strobe- camera) azimuth elevation roll x y z) 

(setf (H-matrix camera) (homogeneous- transform azimuth elevation roll x y z) ) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera)))) 

(defmethod take-picture ( (camera strobe-camera) (body rigid-body) ) 

(let ((camera-space-node-list (mapcar #’ (lambda (node-location) 

(post-multiply ( inverse-H-raatrix camera) node-location)) 

( transformed-node-list body) ) ) ) 

(dolist (polygon (polygon-list body) ) 

(clip-and-draw-polygon camera polygon camera-space-node-list) ) ) ) 

(defmethod clip-and-draw-polygon 

( (camera strobe- camera) polygon node-coord-list) 

(do* ((initial-point (nth (first polygon) node-coord-list)) 

(from-point initial-point to-point) 

( remaining-nodes (rest polygon) (rest remaining-nodes ) ) 

(to-point (nth (first remaining-nodes) node-coord-list) 

(if (not (null (first remaining-nodes))) 

(nth (first remaining-nodes) node-coord-list)))) 

(-(null to-point) 
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(draw-clipped-projection camera from-point initial-point) ) 
(draw-clipped-projection camera from-point to-point) ) ) 

(defmethod draw-clipped-projection ( (camera strobe- camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera)) 

(<= (first to-point) (focal-length camera))) nil) 

((<= (first from-point) (focal-length camera)) 

(draw- line- in- camera-window camera 

(perspective- transform camera (from-clip camera from-point to-point)) 
(perspective-transform camera to-point) ) ) 

((<= (first to-point) (focal-length camera)) 

(draw-line- in-camera-window camera 

(perspective-transform camera from-point) 

(perspective-transform camera (to-clip camera from-point to-point)))) 
(t (draw- line- in-camera-window camera 

(perspective-transf orm camera from-point) 

(perspective-transform camera to-point) ) ) ) ) 

(defmethod from-clip ( (camera strobe- camera) from-point to-point) 

(let ((scale-factor (/ (- (focal-length camera) (first from-point)) 

(- (first to-point) (first from-point))))) 

(list (+ (first from-point) 

(* scale-factor (- (first to-point) (first from-point)))) 

(+ (second from-point) 

(* scale-factor (- (second to-point) (second from-point) ) ) ) 

(+ (third from-point) 

(* scale-factor (- (third to-point) (third from-point)))) 1))) 

(defmethod to-clip ( (camera strobe-camera) from-point to-point) 

(from-clip camera to-point from-point)) 

(defmethod draw- line- in-camera-window ( (camera strobe- camera) start end) 

( cw; draw- line (earner a -window camera) 

(ewimake-position :x (+ 150 (first start) ) 

:y (+ 150 (second start) ) ) 

(ewimake-position :x (+ 150 (first end) ) 

:y (+ 150 (second end) ) ) 

•.brush-width 0) ) 

(defmethod perspective- transform ( (camera strobe -earner a) point-in-camera-space) 
(let* ((enlargement-factor (enlargement- factor camera)) 

(focal-length (focal-length camera) ) 

(x (first point- in-camera-space) ) ;x axis is along optical axis 
(y (second point- in-camera-space) ) ;y is out right side of camera 
(z (third point-in-camera-space))) ;z is out bottom of camera 
(list (+ (round (* enlargement- factor (/ (* focal-length y) x) ) ) 

150) ; to right in camera window 

(+ 150 (round (* enlargement- factor (/ (* focal-length (- z) ) x) ) 

) ) ) ) ) ;up in camera window 

(defun test-camera (z theta) ; Produces top view of default rigid-body. 

(setf robot-leg (make-instance ’rigid-body)) 

(initialize robot-leg) 

(set-transf ormed-node-list -2 robot-leg z) 

( set- transformed-node-list- theta robot-leg theta) 
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(setf camera-1 (make-instance ’strobe-camera)) 

(move camera-1 (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) -30 0 0) 
(take-picture camera-1 robot-leg)) 



(defun deg-to-rad (angle) (* .01745329251994329 angle)) 

(defun animation () 

(do ((theta-loop 0 (+ theta-loop 1))) 

( (> theta-loop 90) ’end) 

(test-camera 30 theta-loop))) 
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TWO LINK INVERTED PENDULUM SIMULATION WITH NEWTON-EULER 



;; File : euler-angle-r igid-body . cl 

;; Author : Dr. R. McGhee 

; ; : Naval Postgraduate School 

; ; : Monterey, CA 93943 

; ; Modified by : Mehmet Bediz 

; ; Summary : This file contains rigid body class which is implemented 

;; : by Dr. R. McGhee. The function "draw-inverted-pendulum" 

; ; ; and the other functions called by this 

; ; : function are implemented by Mehmet Bediz. 

/ / 



(defclass rigid-body 

() 



;The six-vector (u v w p q r) in body coordinates. 
0 0 ) 



((posture ;The vector (xe ye ze phi theta psi) . 

:initform '(000000) 
rinitarg iposture 
: accessor posture) 

(posture-rate ;The vector (xe-dot ye-dot ze-dot phi-dot theta-dot psi-dot) . 
:initarg r posture-rate 
: accessor posture-rate) 

(velocity 

: initform '(000 0 

: initarg :velocity 
: accessor velocity) 

(velocity-growth-rate ;The vector (u-dot v-dot w-dot p-dot q-dot r-dot) . 

: accessor velocity-growth-rate) 

( forces-and- torques ;The vector (Fx Fy Fz L M N) in body coordinates. 

: initform (list 000000) 

: accessor forces-and- torques ) 

(moments-of-inertia ;The vector (Ix ly Iz) in principal axis coordinates, 
linitform '(1 100 1) 

: initarg :moments-of-inertia 
: accessor moments-of-inertia) 

(mass 

: initform 100 
rinitarg rmass 
: accessor mass) 

(node-list ; (x y z 1) in body coord for each node. Starts with (0 0 0 1) . 
linitform '((0 001) (-0.5 -0.5 0.5 1) (0.5 -0.5 0.5 1)(0.5 -0.5 -0.5 1) 

(-0,5 -0.5 -0.5 l)(-0.5 0.5 0.5 1) (0.5 0.5 0.5 1) 

(0.5 0.5 -0.5 l)(-0.5 0.5 -0.5 1) (0 0 0 1)(0 0 3 1) ) 



rinitarg mode-list 
r accessor node-list) 

(polygon-list 

rinitform ' ( ( 1 5 8 4 ) ( 5 6 7 8 ) ( 6 2 3 7 ) ( 2 1 4 3) (9 10)) 
rinitarg rpolygon-list 
r accessor polygon-list) 

( transformed-node-list ; (x y z 1) in earth coord for each node in node-list, 
rinitform ’((0001) (-2 -2 0 1) (2 -2 0 1) (2 -2 -30 l)(-2 -2 -30 1) 

(-2 2 0 1) (2 2 0 1) (2 2 -30 l)(-2 2 -30 1) ) 

: accessor transformed-node-list) 

(H-matrix 
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linitform (unit-matrix 4) 

: accessor H-matrix) 

( time- stamp 

: accessor time-stamp) ) ) 

(defmethod initialize ( (body rigid-body) ) 

(setf ( transf ormed-node-list body) (node-list body) ) 

(setf (velocity-growth-rate body) (update-velocity-growth-rate body) ) 

(setf (posture-rate body) (earth-velocity body) ) 

(setf (time-stamp body) (get- internal-real-time) )) ) 

(defmethod set-transf 02 rmed-node-list-z ( (body rigid-body) z) 

(setf (third (fourth ( transformed-node-list body) ) ) (- z) ) 

(setf (third(fifth (transformed-node-list body))) (- z) ) 

(setf (third (eighth (transformed-node-list body))) (- z) ) 

(setf (third (ninth (transformed-node-list body))) (- z) ) 
(transformed-node-list body))) 

(defun transform (obj) 

(list 0 (first obj) (second obj) (third obj))) 

(defun reverse-transform (obj) 

(list (second obj) (third obj) (fourth obj) 1)) 

(defmethod move ( (body rigid-body) azimuth elevation roll x y z) 

(setf (posture body) (list x y z roll elevation azimuth) ) 

(setf (H-matrix body) 

(homogeneous- transform azimuth elevation roll x y z) ) 

( transf orm-node- list body)) 

(defmethod get-delta-t ((body rigid-body)) 0.005) 

(defmethod f orces-earth-to-body (Fx-e Fy-e Fz-e theta) 

(let* ( ( f orces-earth (list Fx-e Fy-e Fz-e)) 

(R-matrix (rotation-matrix 0 (- theta) 0) ) 

(forces-body (post-multiply R-matrix f orces-earth) ) ) 

forces-body) ) 

(defmethod update-rigid-body ((body rigid-body)) ;Euler integration. 

(let* ( (delta-t (get-delta-t body))) 

(update-posture body delta-t) ; EULER 
(update-velocity body delta-t) ; EULER 

(setf (H-matrix body) (homogeneous -transform (sixth (posture body) ) 

(fifth (posture body)) (fourth (posture body)) (first (posture body)) 
(second (posture body)) (third (posture body)))) 

(transform-node-list body) 

(update-velocity-growth-rate body) ) ) 

(defmethod update-velocity-growth-rate ( (body rigid-body) ) 

(setf (velocity-growth-rate body) ;Assumes principal axis coordinates with 
(multiple-value-bind ; origin at center of gravity of body. 

(Fx Fy Fz L M N u v w p q r Ix ly Iz) ; Declares local variables, 
(values-list ;Values assigned. 
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( append 

( forces-and-torques body) (velocity body) (moments-of-inertia body))) 



(list (+ 


(* 


V r) 


(* - 


1 w q) (/ Fx (mass body)) 






(* 


* gravity* 


(first 


(third (H-matrix body) ) ) ; 


) ) • 


( + 


(* 


w p) 


(* - 


1 u r) (/ Fy (mass body)) 






(* 


* gravity* 


(second 


(third (H-matrix body)); 


) ) ) 


( + 


(* 


u q) 


(* - 


1 V p) (/ Fz (mass body) ) 






(* 


* gravity* 


(third 


(third (H-matrix body) ) ) ] 


) ) 


(/ 


( + 


(^ (- 


ly 


Iz) q r) 


L) Ix) 




(/ 


( + 


(* (- 


Iz 


Ix) r p) 


M) ly) 




(/ 


( + 


{* (- 


Ix 


ly) p q) 


N) Iz))))) ;Value returned 



(defmethod update-velocity ({body rigid-body) delta-t) ;Euler integration. 

(setf (velocity body) 

(vector-add (velocity body) 

(scalar-multiply delta-t (velocity-growth-rate body) ) ) ) ) 

(defmethod update-posture ( (body rigid-body) delta-t) ; Euler integration. 

(setf (posture-rate body) (earth-velocity body)) 

(setf (posture body) 

(vector-add (posture body) (scalar-multiply delta-t (posture-rate body) ) ) ) ) 

(defmethod transform-node-list ( (body rigid-body) ) 

(setf ( transformed-node-list body) 

(mapcar #'( lambda (node-location) 

(post-multiply (H-matrix body) node-location) ) 

(node-list body) ) ) ) 

(defconstant ^gravity* 32.2185) 

(defmethod earth-velocity ((body rigid-body)) 

(let* ((linear-velocity (firstn 3 (velocity body))) 

(rotational-velocity (cdddr (velocity body) ) ) 

(posture (posture body) ) 

(R-matrix (rotation-matrix (sixth posture) (fifth posture) 

(fourth posture) ) ) 

(linear-earth-velocity (post-multiply R-matrix linear-velocity)) 
(T-matrix (body-rate- to-euler-rate-matrix (sixth posture) 

(fifth posture) (fourth posture))) 
(rotational-earth-velocity (post-multiply T-matrix 

rotational-velocity) ) ) 

(append linear-earth-velocity rotational-earth-velocity))) 

(defun test-rigid-body () 

(setf airplane-1 (make- instance 'rigid-body)) 

(initialize airplane-1) 

(setf camera-1 (make-instance 'strobe-camera)) 

(move camera-1 (- (/ pi 2)) 0 0 0 30 0 ) 

(take-picture camera-1 airplane-1) 

(dotimes (i 20 'done) (update-rigid-body airplane-!)) 

(take-picture camera-1 airplane-1)) 
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t f t t i 



two Link Inverted Pendulum 

Massless leg and a single rigid body (fixed leg length) 
state vector x = (theta-1 theta-l-dot theta-2 theta-2-dot) 
state vector u = (M-1 M-2) 

- control torque at the ankle (M-1) 

- control torque at the hip (M-2) 

Newton-Euler Method 

3x3 matrix inversion to calculate forces and moments 
Heun Integration 



(setf 1 0.5) 

(setf r 3) 

(setf time-step 0.01) 

(defun solve- three-equation-system (state-vector K-theta-1 K-theta-l-dot 

K-theta-2 K- theta-2-dot ) 



-1 state-vector K-theta-1 K-theta-l-dot 



(setf 


m 


100) 


(setf 


1 


0.5) 


(setf 


r 


3) 


(setf 


I 


100) 


(setf 


M-1 


(compute 


(setf 


M-2 


(compute 


(setf 


theta-1 


(setf 


theta-l-dot 


(setf 


theta-2 


(setf 


theta-2-dot 


(setf 


g 


32.2) 


(post- 


-multiply 



K-theta-2 K-theta-2-dot) ) 
-vector K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot ) ) 
state-vector) ) 



. ( first 
(second state-vector) ) 
(third state-vector)) 
(fourth state-vector)) 



(matrix- inverse 

(list (list ( 
(list (^ 
V 

(list (^ 



I (*-11 (sin (- 

m r (cos theta-1) ) (* m 

-1 (sin theta-1 ) ) ) 

m r (sin theta-1)) (* m 



theta-2 theta-1)))) 
1 (cos theta-2)) 

1 ( sin theta-2 ) ) 



(cos theta-1 ) ) ) ) 



(list (/ (* (- (* M-2 (+ 1 r)) (* 1 M-1) ) (cos (- theta-2 theta-1))) r) 

(+ (* m r theta-l-dot theta-l-dot (sin theta-1) ) 

(* m 1 theta-2-dot theta-2-dot (sin theta-2)) 

(/ (* (- M-1 M-2) (cos theta-1) ) r) ) 

(+ (* -1 m r theta-l-dot theta-l-dot (cos theta-1)) 

(* -1 m 1 theta-2-dot theta-2-dot (cos theta-2)) 

(* m g) 

(/ (* (- M-1 M-2) (sin theta-1)) r) ) ) ) ) 



(defun euler-step-constant-leg (state-vector threeXthree-matrix) 
(mapcar '+ state-vector (scalar-multiply time-step 

( derivative- state- vector 
state-vector threeXthree-matrix) ) ) ) 



108 



(defun heun-step-constant-leg (x K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot ) 

(setf threeXthree-matrix (solve- three-equation-system 

X K-theta-1 K-theta-l-dot 
K-theta-2 K-theta-2-dot) ) 

(mapcar '+ x ( scalar-multiply (* time-step .5) 

(vector-add (derivative-state-vector x threeXthree-matrix) 

( derivative-state-vector 

(euler-step-constant-leg x threeXthree-matrix) 
threeXthree-matrix) ) ) ) ) 

(defun derivative-state-vector (state-vector threeXthree-matrix) 
(list (second state-vector) 

(first threeXthree-matrix) 

(fourth state-vector) 

(second threeXthree-matrix) ) ) 



(defun compute-M-1 (state-vector K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot) 

(setf theta-1 (first state-vector)) 

(setf theta-l-dot (second state-vector)) 

(+ (* -1 K-theta-1 theta-1) (* -1 K-theta-l-dot theta-l-dot))) 

(defun compute-M-2 (state-vector K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot ) 

(setf theta-2 (third state-vector)) 

(setf theta-2-dot (fourth state-vector)) 

(+ (* -1 K-theta-2 theta-2) (* -1 K- theta-2-dot theta-2-dot) ) ) 

(defun compute-y (state-vector) 



(setf 


theta- 1 


( first 


state-vector) ) 


(setf 


theta-l-dot 


(second 


state-vector) ) 


(setf 


theta-2 


(third 


state-vector) ) 


(setf 


theta-2-dot 


(fourth 


state-vector) ) 


(setf 


1 0.5) 






(setf 


r 3) 






(+ (* 


r (sin theta- 


1) ) 




(* 


1 (sin theta- 


2) ) ) ) 




(defun compute-z (state-vector) 




(setf 


theta- 1 


( first 


state-vector) ) 


(setf 


theta-l-dot 


(second 


state-vector) ) 


(setf 


theta-2 


(third 


state-vector) ) 


(setf 


theta-2 -dot 


( fourth 


state-vector) ) 


( setf 


1 0.5) 






(setf 


r 3) 







(+ (* r (cos theta-1) ) 

{* 1 (cos theta-2)))) 



(defun draw- inverted-pendulum (halt-time K-theta-1 K-theta-l-dot 

K-theta-2 K-theta-2-dot ) 
(setf robot-body (make-instance 'rigid-body)) 
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(initialize robot-body) 



(setf robot-leg (make- ins tance ’rigid-body)) 
(initialize robot-leg) 



(setf camera-1 (make- instance ’ strobe- camera ) ) 

(move camera-1 (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) -10 0 0) 

(setf graph- 1-window (make-window-graph- 1) ) 

(draw- coordinate-axes graph- 1 -window) 

(setf graph- 2 -window ( make- window- graph- 2 ) ) 

(draw-coordinate-axes graph- 2 -window) 

(do ((x '(0.5 0 1.0 0) (heun-s tep-cons tant-leg x K- theta-1 K- theta-l-dot 

K-theta-2 K-theta-2-dot) ) 

(old-x ' (0.5 0 1.0 0) X) 

(time 0 (+ time 1) ) ) 

( (> time halt-time) ’done) 



(move robot-body (deg-to-rad 0) (deg-to-rad 0) (third x) 

0 (compute-y x) (* -l(compute-z x) ) ) 



(setf ( transf ormed-node-list robot-leg) (list (list 0001) 

(list 0 (* r (sin (first x) ) ) 
(* -1 r (cos (first x) ) ) 1) 
(list 0001))) 

(setf (polygon-list robot-leg) (list (list 2 1))) 

(cw: clear (camera-window camera-1)) 



(take-picture camera-1 robot-body) 
(take-picture camera- 1 robot-leg) 

(draw- line- in-window graph- 1-window 80 (list 

( list 

(draw- line- in-window graph- 2 -window 80 (list 

(list 



(* time- step (- time 1) ) 
(first old-x)) 

time-step time) 

(first X) ) ) 

(* time-step (- time 1) ) 
(third old-x) ) 

(^ time- step time) 

(third x) ) ) ) ) 



(defun make-window-graph-1 () 

(cw: make- window- stream ; borders 5 



:left 0 
‘.bottom 50 
‘.width 450 
:height 450 

: title "Leg Angle (theta-1) NEWTON-EULER" 
:activate-p t) ) ;Make window visible. 



(defun make-window-graph-2 () 

(cw: make- window- stream : borders 5 

:left 0 
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bottom 550 



width 450 
height 450 

title "Body Attitude (theta-2) NEWTON-EULER" 
activate-p t) ) ;Make window visible. 



Ill 



★ *■*★★★★*★*•**■*■**★★*★*■*★•*****★*★★■***★★*★★★*★*****★★*★★★*****★******** 



File 

Author 



SuiTimary 



load-euler-f lies . cl 
Mehmet Bediz 

Naval Postgraduate School 
Monterey, CA 93943 

This file contains functions to load files. 



r*******************************************************************^ 



; Load files 






(load "harmonic-equation. cl ” ) 

(load " robot-kinematics . cl " ) 

( load " euler-angle-rigid-body . cl “ ) 
(load " strobe- camera, cl '• ) 
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File 

Author 



★★★★★★★★ 






: strobe-camera. cl 
: Dr. R. McGhee 
: Naval Postgraduate School 
: Monterey, CA 93943 
;; Sununary : This file contains strobe -camera class definitions. 

(require :xcw) 

(cw: initialize-common-windows) 

(defclass strobe-camera (rigid-body) 

( ( focal-length 

laccessor focal-length 
rinitform 6) 

(camera- window 
: accessor camera-window 

rinitform (cw:make-window-stream ; borders 5 

:left 470 
ibottom 500 
:width 650 
:height 650 

: title "Two Link Inverted Pendulum 

(Constant Length Massless Leg) 
NEWTON-EULER" 

: activate-p t ) ) 

(H-matrix 

rinitform (homogeneous-transform .3 -.3 0 -300 -90 -90)) 

( inverse-H-matrix 
: accessor inverse-H-matrix 

rinitform (inverse-H (homogeneous-transform .3 -.3 0 -300 -90 -90))) 
(enlargement -factor 
r accessor enlargement-factor 
: initform 30) ) ) 

(defmethod move ((camera strobe-camera) azimuth elevation roll x y z) 

(setf (H-matrix camera) (homogeneous-transform azimuth elevation roll x y z) ) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera)))) 

(defmethod take-picture ((camera strobe-camera) (body rigid-body)) 

(let ( (camera-space-node-list (mapcar #' (lambda (node-location) 

(post-multiply (inverse-H-matrix camera) node-location)) 
(transformed-node-list body) ) ) ) 

(dolist (polygon (polygon-list body) ) 

(clip-and-draw-polygon camera polygon camera-space-node-list ) ) ) ) 

(defmethod clip-and-draw-polygon 

((camera strobe-camera) polygon node-coord-list) 

(do* ((initial-point (nth (first polygon) node-coord-list)) 

(from-point initial-point to-point) 

( remaining-nodes (rest polygon) (rest remaining-nodes) ) 

(to-point (nth (first remaining-nodes) node-coord-list) 

(if (not (null (first remaining-nodes))) 

(nth (first remaining-nodes) node-coord-list)))) 

( (null to-point) 

(draw-clipped-projection camera from-point initial-point)) 
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(draw-clipped-projection camera from-point to-point) ) ) 



(defmethod draw-clipped-projection ( (camera strobe -camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera)) 

(<= (first to-point) (focal-length camera))) nil) 

( (<= (first from-point) (focal-length camera)) 

(draw- line- in-camera-window camera 

(perspective-transform camera (from-clip camera from-point to-point)) 
(perspective-transform camera to-point) ) ) 

( (<= (first to-point) (focal-length camera)) 

( draw- 1 ine- in-camera-window camera 

(perspective-transf orm camera from-point) 

(perspective-transform camera (to-clip camera from-point to-point)))) 
(t (draw- line- in- camera-window camera 

(perspective-transf orm camera from-point) 

(perspective-transform camera to-point) ) ) ) ) 

(defmethod from-clip ( (camera strobe-camera) from-point to-point) 

(let ((scale-factor (/ (- (focal-length camera) (first from-point)) 

(- (first to-point) (first from-point))))) 

(list (+ (first from-point) 

(* scale-factor (- (first to-point) (first from-point)))) 

(+ (second from-point) 

(* scale-factor (- (second to-point) (second from-point)))) 

(+ (third from-point) 

(* scale-factor (- (third to-point) (third from-point)))) 1))) 

(defmethod to-clip ((camera strobe- camera) from-point to-point) 

(from-clip camera to-point from-point)) 

(defmethod draw- line- in- camera-window ((camera strobe-camera) start end) 

(cw: draw- line ( earner a- window camera) 

(ewimake-position :x (+ 150 (first start)) 

:y (+ 150 (second start) ) ) 

(ewimake-position :x (+ 150 (first end) ) 

:y (+ 150 (second end) ) ) 

: brush-width 0) ) 

(defmethod perspective-transform ( (camera strobe- camera) point-in-camera-space) 
(let* ((enlargement-factor (enlargement- factor camera)) 

(focal-length (focal-length camera)) 

(x (first point-in-camera-space)) ;x axis is along optical axis 
(y (second point-in-camera-space) ) ;y is out right side of camera 
(2 (third point-in-camera-space))) ;z is out bottom of camera 
(list (+ (round (* enlargement-factor (/ (* focal-length y) x) ) ) 

150) ;to right in camera window 

(+ 150 (round (* enlargement- factor (/ (* focal-length (- z) ) x) ) 

) ) ) ) ) ; up in camera window 



(defun test-camera (z theta) ; Produces top view of default rigid-body, 
(setf robot-leg (make-instance ’rigid-body)) 

(initialize robot-leg) 

(set-transformed-node-list -2 robot-leg z) 
(set-transformed-node-list-theta robot-leg theta) 

(setf camera-1 (make- instance ’strobe-camera)) 
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(move camera-1 (deg-to-rad 0) (deg-to-rad 0) (deg-to-rad 0) -30 0 0) 
(take-picture camera-1 robot-leg)) 



(defun deg-to-rad (angle) (* .01745329251994329 angle)) 

(defun animation () 

(do ((theta-loop 0 (+ theta-loop 1))) 

( (> theta- loop 90) ’end) 

(test-camera 30 theta-loop))) 
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APPENDIX B: KINEMATIC SIMULATION SOFTWARE 



^y3|i:**4:***:4c***:4c3k***4c******9t(9ic**3(c******4:*4:**3ii:***3|c4:*3|<:3ic:>ic*9i(*9|<:9ic***3jc:*9)e4:3jc*3)c3)c**3(;* 

// File : main.C 

// Author : Mehmet Bediz 

// : Naval Postgraduate School 

// : Monterey, CA 93943 

//Created : November 1996 

// Summary : This file contains the main function and three motion 

// : functions of the kinematic model Dynaman: forward stepping, 

// : upward stepping, and jumping. 

^^*9|c**3|i:*****9|c****** *:!(** ****** **:jc****:i;******:t:* ******************** ****** 



#include <string.h> 

#include "main.h" 

void step_forward(int number_of_steps, float X, float Y, float Z); 
void step_upward(int number_of_steps, float X, float Y, float Z); 
void jump(float X,float Y,float Z); 

// Speed of the animation can be controlled by changing delta_t 
float delta_t = 2 * 3.0; 



pfNode 


*root; 


pfDCS 


*dcs; 


pfMatrix 


mat, orbit; 


pfSphere 


sphere; 


pfNode 


*modell, *model2, *model3, *model4; 


pfNode 


* models, *modellO; 


pfNode 


*modelll, *modell2, *modell3, *modell4; 


pfDCS 


*nodel 1, *nodel2, *nodel3, *nodel4; 


pfDCS 


*nodel, *node2, *node3, *node4; 


pfDCS 


*nodeS, *nodel0; 


pfDCS 


*dcs0; 


pfDCS 


*dcsl, *dcs2, *dcs3; 


pfDCS 


*dcs4, *dcs5, *dcs6; 


pfDCS 


*dcs7, *dcs8, *dcs9, *dcsl0; 


pfDCS 


*dcsl 1, *dcsl2, *dcsl3; 


pfDCS 


*dcsl4, *dcsl5, *dcsl6; 


pfDCS 


*dcs30, *dcs31, *dcs32, *dcs33; 


pfDCS 


*dcs34, *dcs35, *dcs36; 


pfDCS 


*dcs37, *dcs38, *dcs39; 


char 


*filel,*file2, *file3, *file4; 


char 


*file8, *filel0; 


char 


*filell, *filel2, *filel3, *filel4; 


pfLightSource *light; 


pfMatrix 


lightMat; 


float 


d; 


pfTexture 


*tex; 


pfFrustum 


*Frust; 


void 


* arena; 


pfDCS 


*lightDCS; 
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int 

float 



i; 

finaLposition; 



//■ 

// 

// 

// 

// 

// 

// 

// 

// 

//- 



Function: 

Returns: 

Parameters: 

Summary: 



main 

None 

None 

Initializes IRIS Performer™, loads body parts from poly format 
files to DCS nodes, creates the channels at the upper right 
comer of the window, calls step_forward, step_upward, and 
jump functions with the number of steps and the initial 
positions. 



void 

main (int argc, char *argv[]) 

{ 



/* choose default objects of none specified */ 

filel = "lowerleg.poly"; 

file2= "upperleg.poly"; 

fileS = "head.iv”; 

file4= "torso.poly"; 

fileS = "foot.poly"; 

filelO = "floor.iv"; 

filel 1 = "rightupperarm.poly"; 

filel2 = "lowerarm.poly"; 

file 13 = "hand.poly"; 

file 14 = "leftupperarm.poly"; 

if ( ! strcmp(argv[l],"slow")){ 
delta_t = 2 * 0.5; 

) 

#ifndefIRISGL 

printfC’Sorry, shadows doesn't work in OPENGL\n"); 
return 0; 

#endif 

/* Initialize Performer */ 
pfInitO; 

/* Use default multiprocessing mode based on number of 

* processors. 

*l 

pfMultiprocess(PFMP_DEFAULT); 

/** allocate shared memory **/ 

InitSharedO; 

/* Configure multiprocessing mode and start parallel 

* processes. 

*/ 

pfConfigO; 
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/** Initialize Performer utility and GUI functions **/ 
pfuInitUtilQ; 

pfInitClock(0.00; 

Shared->simTime = pfGetTimeO; 
srand(Shared->simTime* 1 0000000); 

/* Append to PFPATH additional standard directories where 

* geometry and textures exist 
*/ 

pfFilePath(”.:/usr/share/Performer/data:/tmp_mnt/workb/bediz/Performer/data"); 

/* Do not use FLAT_ primitives because they look bad 

* with local lighting. 

*/ 

pfdBldrMode(PFDBLDR_MESH_LOCAL_LIGHTING, 1 ); 

/* Read a single file, of any known type. */ 
if ((root = pfdLoadFileC'text.iv")) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

/* Load the files */ 

if ((model 1 = pfdLoadFile(filel)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((model2 = pfdLoadFile(file2)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((models = pfdLoadFile(fileS)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((model4 = pfdLoadFile(file4)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((models = pfdLoadFile(fileS)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((model 10 = pfdLoadFile(filelO)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 
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if ((model 1 1 = pfdLoadFile(filel 1)) = NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((model 12 = pfdLoadFile(filel2)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((model 13 = pfdLoadFile(filel3)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 

if ((modell4 = pfdLoadFile(filel4)) == NULL) 

{ 

pfExitO; 

exit(-l); 

} 



/* Create and attach loaded subgraph to a pfScene. */ 
Shared->scene = new pfScene; 
dcs = new pfDCS; 
dcs->addChild(root) ; 

Shared->scene->addChild(dcs); 

/* determine extent of scene's geometry */ 
Shared->scene->getBound(&(Shared->bsphere)); 



/******************* MAIN WINDOW ****************/ 

/* Configure and open GL window */ 

Shared->p = pfGetPipe(O); 

Shared->p2 = pfGetPipe(O); 

Shared->pw = new pfPipeWindow(Shared->p); 
Shared->pw->setName("DYNAMAN"); 
Shared->pw->setConfigFunc(OpenPipeWin); 
Shared->pw->setOriginSize(120, 120, 1100, 1100); 
Shared->pw->config(); 

/* initializes mouse and keyboard inputs to be read from window */ 
pfuInitInput(Shared->pw, PFUINPUT_GL); 
pfuInitGUI(Shared->pw); 
pfuEnableGUI(TRUE); 

/* Create and configure a pfChannel. */ 

Shared->chan = new pfChannel(Shared->p); 
Shared->chan->setScene(Shared->scene); 
Shared->chan->setNearFar(1.0f, 5.0f * Shared->bsphere.radius); 
Shared->chan->setFOV(45.0f, 0.00; 

Shared->view.xyz.set(1.3f * Shared->bsphere.radius, 

-2.5 f* Shared->bsphere.radius, 

3.0f * Shared->bsphere. radius); 
Shared->view.hpr.set(0.0f, -45. Of, 0.00; 

Shared->chan->setView(Shared->view.xyz, Shared->view.hpr); 
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Shared->chan->setTravFunc(PFTRAV_DRAW,DrawChannel); 

y******** FIRST CHANNEL **************************/ 
Shared->chan2[0] = new pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[0]); 
Shared->chan2[0]->setTravFunc(PFTRAV_DRAW,DrawChanneI); 
Shared->chan2[0]->setScene(Shared->scene); 
Shared->chan2[0]->setNearFar(L0f, 5.0f * Shared->bsphere.radius); 
Shared->chan2[0]->setFOV(45.0f, O.Of); 

Shared->view.xyz.set( Shared->bsphere. radius, 

-4.0f * Shared->bsphere.radius, 

0.8f * Shared->bsphere.radius); 
Shared->view.hpr.set(O.Of, O.Of, O.Of); 

Shared->chan2[0]->setView(Shared->view.xyz, Shared->view.hpr); 

y* ****** * SECOND CHANNEL **************************/ 

Shared->chan2[l] = new pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[l]); 

Shared->chan2[ 1 ]->setTravFunc(PFTRA V_DRAW,DrawChanneI); 
Shared->chan2[l]->setScene(Shared->scene); 
Shared->chan2[l]->setNearFar(i.0f, 5.0f * Shared->bsphere.radius); 
Shared->chan2[l]->setFOV(45.0f,0.0f); 

Shared->view.xyz.set(1.3 * Shared->bsphere.radius, 

-2.5f * Shared->bsphere.radius, 

3. Of * Shared->bsphere.radius); 
Shared->view.hpr.set(O.Of, -45.0f, O.Of); 

Shared->chan2[l]->setView(Shared->view.xyz, Shared->view.hpr); 

/******** third channel **************************/ 

Shared->chan2[2] = new pfChannel(Shared->p); 

Shared->pw->addChan(Shared->chan2[2]); 

Shared->chan2[2]->setTravFunc(PFTRAV_DRAW,DrawChannel); 

Shared->chan2[2]->setScene(Shared->scene); 

Shared->chan2[2]->setNearFar(1.0f, 5.0f * Shared->bsphere. radius); 

Shared->chan2[2]->setFOV(45.0f,0.0f); 

Shared->view.xyz.set( 1.6 * Shared->bsphere. radius, 

-0.15f * Shared->bsphere. radius, 

2.5f * Shared- >bsphere. radius); 
Shared->view.hpr.set(O.Of, -90.0f, O.Of); 

Shared->chan2[2]->setView(Shared->view.xyz,Shared-> view.hpr); 

y******** POURTH CHANNEL **************************/ 

Shared->chan2[3] = new pfChannel(Shared->p); 
Shared->pw->addChan(Shared->chan2[3]); 

Shared->chan2[3]->setTravFunc(PFTRAV_DRAW,DrawChannel); 
Shared->chan2[3]->setScene(Shared->scene); 
Shared->chan2[3]->setNearFar(1.0f, 5.0f * Shared->bsphere.radius); 
Shared->chan2[3]->setFOV(45.0f,0.0f); 

Shared->view.xyz.set( 4.0f * Shared->bsphere.radius, 

-0.5f * Shared->bsphere.radius, 

0.5 * Shared->bsph ere. radius); 
Shared->view.hpr.set(90.0f, O.Of, O.Of); 
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Shared->chan2[3]->setView(Shared->view.xyz, Shared->view.hpr) 
Shared->chan2[3]->setViewport(0.7, 0.85, 0.7, 0.85); 
Shared->chan2[2]->setViewport(0.85, 1.0, 0.7, 0.85); 
Shared->chan2[l]->setViewport(0.85, 1.0, 0.85, 1.0); 
Shared->chan2[0]->setViewport(0.7, 0.85, 0.85, 1.0); 

orbit.makeRot(1.0f, O.Of, O.Of, l.Of); 

/* scale models to unit size */ 
nodel = new pfDCS; 
node 1 ->addChild( model 1 ); 
model 1 ->getBound(&sphere); 
if (sphere.radius > O.Of) 

node 1 ->setScale( 1 .Of/sphere.radius); 

node2 = new pfDCS; 
node2->addChild(model2); 
model2->getBound(&sphere); 
if (sphere.radius > O.Of) 

node2->setScale( 1 .Of/sphere.radius); 

node3 = new pfDCS; 
node3->addChild(model3); 
model2->getBound(&sphere); 
if (sphere.radius > O.Of) 

node3->setScale( 1 .Of/sphere.radius); 

node4 = new pfDCS; 
node4->addChild(model4); 
model4->getBound(&sphere); 
if (sphere.radius > O.Of) 

node4->setScale( 1 .Of/sphere.radius); 

node8 = new pfDCS; 
node8->addChild(model8); 
model8->getBound(&sphere); 
if (sphere.radius > O.Of) 

node8->setScale(l. Of/sphere, radius); 

node 10 = new pfDCS; 
node 1 0->addChi ld(model 1 0) ; 
model 1 0->getBound(&sphere); 
if (sphere.radius > O.Of) 

nodelO->setScale( 1 .Of/sphere.radius); 

nodell = new pfDCS; 
node 1 1 ->addChild(model 11); 
model 1 l->getBound(&sphere); 
if (sphere.radius > O.Of) 

node 1 0->setScale( 1 .Of/sphere.radius) ; 

node 12 = new pfDCS; 
node 1 2->addChild(model 1 2); 
model 1 2->getBound(&sphere); 
if (sphere.radius > O.Of) 
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node 1 0->setScale( 1 .Of/sphere. radius); 



node 13 = new pfDCS; 
node 1 3->addChild(model 1 3); 
model 1 3->getBound(&sphere); 
if (sphere.radius > O.Of) 

node 1 0->setScale( 1 .Of/sphere.radius); 

node 14 = new pfDCS; 
nodel4->addChild(modell4); 
model 14->getBound(&sphere); 
if (sphere.radius > O.Of) 

node 1 0->setScale( 1 .Of/sphere.radius); 

/* Lighting without shadowing */ 

// put a default light source in the scene 
Shared->scene->addChild(new pfLightSource); 

arena = pfGetSharedArena(); 

// Create and configure shadow frustum. Fit frustum tightly to scene. 
Frust = new(arena) pfFrustum; 

Frust->makeSimple(FOV); 

d = Shared->bsphere.radius / sinf(PF_DEG2RAD(FOV/2.0f)); 
d = PF_MAX2(d, NEAR + Shared->bsphere.radius); 

Frust->setNearFar(NEAR, d + l.lf * Shared- >bsphere.radius); 
tex = initSpotTexO; 

light = new pfLightSource; 
light->setMode(PFLS_PROJTEX_ENABLE, 1 ); 
light->setAttr(PFLS_PROJ_FRUST, Frust); 
li gh t->set Attr(PFLS_PROJ_TEX , tex) ; 
light->setColor(PFLT_DlFFUSE, l.Of, l.Of, l.Of); 
light->setVal(PFLSJNTENSITY, 3.0f); 
light->setPos(17.0 , -7.0 , 13.9, i.OQ;// Make light local 
light->setSpotDir(O.Of, O.Of, -lO.Of); 

// Make DCS to move lights around 
lightDCS = new pfDCS; 
lightDCS->addChild(light); 

/* Main DCS */ 
dcsO = new pfDCS; 

I* Left Foot */ 
dcsl = new pfDCS; 
dcsl -> addChild(nodel); 

/* Left lowerleg */ 

!* Right lowerleg */ 
dcs4 = new pfDCS; 
dcs4 -> addChild(nodel); 
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/* Left upperleg */ 
dcs2 = new pfDCS; 
dcs2 -> addChild(node2); 

/* Right upperleg */ 
dcs5 = new pfDCS; 
dcs5 -> addChild(node2); 

/* Floor */ 
dcslO = new pfDCS; 
dcslO -> addChild(nodelO); 
dcs30 = new pfDCS; 
dcs30 >> addChild(nodelO); 
dcs31 = new pfDCS; 
dcs31 -> addChild(nodelO); 
dcs32 = new pfDCS; 
dcs32 >> addChild(nodelO); 
dcs33 = new pfDCS; 
dcs33 -> addChild(nodelO); 
dcs34 = new pfDCS; 
dcs34 -> addChild(nodelO); 
dcs35 = new pfDCS; 
dcs35 -> addChild(nodelO); 
dcs36 = new pfDCS; 
dcs36 -> addChild(nodelO); 
dcs37 = new pfDCS; 
dcs37 -> addChild(nodelO); 
dcs38 = new pfDCS; 
dcs38 -> addChild(nodelO); 
dcs39 = new pfDCS; 
dcs39 -> addChild(nodelO); 

/* Torso (Two parts) */ 
dcs6 = new pfDCS; 
dcs6 -> addChild(node4); 
dcsO -> addChild(dcs6); 

dcs3 = new pfDCS; 
dcs3 -> addChild(node4); 
dcsO -> addChild(dcs3); 

/* Left Legs */ 
dcs2 -> addChild(dcsl ); 
dcsO -> addChild(dcs2); 

/* Right Legs */ 
dcs5 -> addChild(dcs4); 
dcsO -> addChild(dcs5); 

/* Feet*/ 
dcs8 = new pfDCS; 
dcs8 -> addChild(node8); 
dcs9 - new pfDCS; 
dcs9 -> addChild(node8); 
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dcsl -> addChild(dcs8); 
dcs4 -> addChild(dcs9); 

/* Head */ 
dcs7 = new pfDCS; 
dcs7 -> addChild(node3); 
dcs3 -> addChild(dcs7); 

/* Right upper arm */ 
dcsl 1 = new pfDCS; 
dcsl 1 -> addChild(nodel 1); 
dcsO -> addChild(dcsl 1); 

/* Right lower arm */ 
dcsl 2 = new pfDCS; 
dcsl 2 -> addChild(nodel2); 
dcsll -> addChild(dcsl2); 

/* Right hand */ 
dcsl 3 = new pfDCS; 
dcsl 3 -> addChild(nodel3); 
dcsl2-> addChild(dcsl3); 

/* Left upper arm */ 
dcsl 4 = new pfDCS; 
dcsl 4 -> addChild(nodel4); 
dcsO -> addChild(dcsl4); 

/* Left lower arm */ 
dcsl5 = new pfDCS; 
dcsl 5 -> addChild(nodel2); 
dcsl4>> addChild(dcsl5); 

/* Left hand */ 
dcsl 6 = new pfDCS; 
dcsl6 -> addChild(nodel3); 
dcsl5-> addChild(dcsl6); 

// Stairs 

dcs30 -> addChild(dcs31); 
dcs31 -> addChild(dcs32); 
dcs32 -> addChild(dcs33); 
dcs33 -> addChild(dcs34); 
dcs34 -> addChild(dcs35); 
dcs35 -> addChild(dcs36); 
dcs36 -> addChild(dcs37); 
dcs37 -> addChild(dcs38); 
dcs38 -> addChild(dcs39); 

Shared->scene-> addChild(dcsO); 
Shared->scene-> addChild(dcs30); 

/* Create "floor letters" */ 
dcs -> addChild(initFloor(&Shared->bsphere)); 



/* Floor */ 
dcsl 0->setScale(2.0f); 
dcsl0->setRot(0.0, -90.0 ,0.0); 
dcsl0->setTrans(-1.0 , -11.0 , 0.05); 

// Draw steps 
dcs3 0->setScale(0. 5 f); 
dcs30->setRot(-90.0, -90.0 ,0.0); 
dcs30->setTrans(13.0 ,0.0,0.05); 
dcs31->setTrans(0.0 , -2.0 , 5.4); 
dcs32->setTrans(0.0 ,-2.0, 4.1); 
dcs33->setTrans(0.0 ,-1.9, 4.1); 
dcs34->setTrans(0.0 , - 1 .9 , 4. 1 ); 
dcs35->setTrans(0.0 ,-1.9, 4.1); 
dcs36->setTrans(0.0 , - 1 .8 , 4. 1 ); 
dcs37->setTrans(0.0 ,-1.8, 4.1); 
dcs38->setTrans(0.0 , -1.8 , 4.1); 
dcs39->setTrans(0.0 , -1.7 , 4.1); 

/* Set up initial/default view */ 

MakeGUIO; 

int temp = system("sfplay runaway.wav &"); 
if (-1 = temp){ 
printf(” Can’t play \n”); 

f 

else{ 

printf("Playing %i \n’’,temp); 

} 

for (int forever = 0; forever < 50; forever ++){ 
step_forward(6, -9.8 , -7.0 , 3.9); 
step_upward(5, 13.3 , -7.0 ,3.9); 
jump(31.7, -7.0,12.496556); 
step_forward(4, 39.137990 , -7.0 , 4.670563); 

} 

// Kill music 
system("music_killer”); 

/* Terminate parallel processes and exit. */ 
pfExitO; 
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Returns: 

Parameters: 

Summar>': 



step_foruard 

None 

Number of steps needs to be taken, initial position 
Computes the joint angles according to the position 
of the end effector(foot) by using the inverse kinematic 
equations of the three link planar manipulator. The 
algorithm for the path of the foot is described in 
Chapter IV of this thesis as “Stepping Forward Algorithm”. 



void step_foru'ard(int number_of_steps,float X, float Y,float Z){ 

float 11 = l.Of; 
float 12 = 1. Of; 
float cl_left,cl_right; 
float theta l,theta2; 
float kl_left,k2_left; 
float kl_right,k2_right; 

float xjeft= 1.732f; 
float yjeft = O.Of; 
float x_right = 1.732f; 
float y_right = O.Of; 

// INITIAL HALF STEP 
for (float z = 32 ; z < 43 ; z += 0.5 * delta_t) 

{ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime = pfGetTime(); 

/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/* Head *! 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 

/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 
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Left lowerleg */ 
dcs4->setTrans(0 ,-1.7,0); 



/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 
dcsl l->setTrans(-1.0 , 1.7, 0); 

/* Right lower aim */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6 ,1.8,0); 

/* Left lower arm */ 
dcsl5->setRot(0, 0 , 180.0); 
dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

H Inverse Kinematics 

// 

I I SECOND STEP 
// RIGHT LEG STEP 

x_right = 0.95f * cos((64 - 2* z) * DEG_TO_RAD )* 

(11 + 12) * cos((64 - 2* z) * DEG_TO_RAD); 
y.right = 0.95f * cos((64 - 2* z) * DEG_TO_RAD )* 

(II + 12) * sin((64 - 2* z) * DEG_TO^RAD); 

cl_right= ((x_right*x_right) + (y_right*y_right) 

- (11*11) -(12*12))/(2* II *12); 
theta2 = (1 *acos(cl_right)); 

kl fright = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

theta 1 = atan(y_right/x_right) - atan(k2_right/kl_right); 
// Right Leg 

dcs2->setRot(0 ,(57.3 * thetal),0); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 
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// Left Arm 

dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.00,0 ); 
dcsl5->setRot(0 ,(L5f *(57.3 * thetal) - 10.00, 0); 

// LEFT LEG STEP 

xjeft = cos((-64 + 2* z) * DEG_TO_RAD ) * (11 + 12) * 
cos((-64 + 2* z) * DEG_TO_RAD); 
yjeft = cos((-64 + 2* z) * DEG_TO^RAD ) * (11 + 12) * 
sin((-64 + 2* z) * DEG_TO_RAD); 
cl_left= ((x_left*x_lefl) + (y_left*y_left) 

- (11*11) -(12*l2))/(2* 11 *12); 
theta2 = (l *acos(cl_left)); 

kl_left = 11 4* (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

thetal = atan(y_left/x_left) - atan(k2_left/kl_left); 

//Left leg 

dcs5->setRot(0 ,(57.3 * thetal), 0); 
dcs4->setRot(0 ,(57.3 * theta2), 0); 

// Right arm 

dcsl l->setRot(0 ,(0.7f *(57.3 * thetal) + 10.00,0); 
dcsl2->setRot(0 ,(1.5f *(57.3 * thetal) - 10.00,0); 

//Torso rotation 
if(z<31){ 

dcs3->setRot((-(z - 21)/3.0),0 ,0); 
dcs6->setRot((-(z -21)/3.0),0 ,0); 

) 

else{ 

dcs3->setRot(((-20 + ((z- 21)))/3.0),0 ,0); 
dcs6->setRot(((-20 + ((z- 21)))/3.0),0 ,0); 

} 

dcsO->setTrans(X + (z - 32) * 0.1 ,Y, 

(Z + 0.2) + 0.08f * cos(2.0f* 3.14159f * (z - 41) / 21.00); 

UpdateViewO; 

UpdateGUIO; 

pfFrameO; 

)//End of second for 

for( i = 1 ; i < number_of_steps ; i++){ 

for ( z = (i-l)*42 + 1 ; z < (i-l)*42 + 22 ; z += 0.5 * delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime=pfGelTime(); 



/* Main Body */ 
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dcs0->setRot(90.0, 90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 

dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setXrans(0.3 ,2.1,0.005); 

/* Left foot */ 
dcs9->setScale(0.7f) ; 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setXrans(0 ,-1.7,0); 

/* Left lowerleg */ 
dcs4->setXrans(0 , -1.7, 0); 

/* Left upperleg */ 
dcs5->setXrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setXrans(0 , -1.7, 0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 
dcsl l->setXrans(-1.0 , 1.7,0); 

/* Right lower arm */ 
dcsl2->setXrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setXrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcsl4->setScale(0.25f); 
dcsl4->setXrans(1.6 ,1.8,0); 

/* Left lower arm */ 
dcsl5->setRot(0, 0 , 180.0); 
dcsl5->setXrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setXrans(-0.2 , -4.6, 0.0); 



llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
// Inverse Kinematics 

lllllllllllllllllllllllllllllllinillUIIIIIIIIIIIIIIH 

U FIRST STEP 
// LEFT LEG 

xjeft = 0.95f * cos((22 - 2* (z- 42 *(i - 1))) * DEG.TO^RAD ) 

*(11 + 12) * cos((22 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD); 

yjeft = 0.95f * cos((22 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD ) 

*(11 + 12) * sin((22 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD); 

cl_left= ((x_left*x_left) + (yjeft*yjeft) - (11*11) 
-(12*12))/(2*11 *12); 
lheta2 = (1 *acos(cl_left)); 
kljeft = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

theta 1 = atan(yJeftZxJeft) - atan(k2_left/kl Jeft); 

dcs5->setRot(0 ,(57.3 * theta 1),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 

// Right Arm 

dcsl l->setRot(0 ,(0.7f *(57.3 * thetal) + lO.OO.O ); 
dcsl2->setRot(0 ,(1.5f *(57.3 * thetal) - 10.00, 0); 

// Right Leg 

x_right = cos((-22 + 2 * (z- 42 *(i - 1))) * DEG_TO_RAD ) 

* (11 + 12) * cos((-22 + 2 * (z- 42 *(i - 1))) * DEG_TO_RAD); 
y.right = cos((-22 + 2 * (z- 42 *(i - 1))) * DEG_TO_RAD ) 

* (11 + 12) * sin((-22 + 2 * (z- 42 *(i - 1))) * DEG_TO_RAD); 
cl_right= ((x_right*x_right) + (y_right*y_right) 

- (11*11) -(12*12))/(2* 11 *12); 
theta2 = (1 *acos(cl_right)); 

kl_right = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

thetal = atan(y_right/x_right) - atan(k2_right/kl_right); 

dcs2->setRot(0 ,(57.3 * thetal), 0); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 

// Left Arm 

dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.00,0); 
dcsl5->setRot(0 ,(1.5f *(57.3 * thetal) - 10.00,0); 

// Torso rotation 
if((z- 42*(i- 1))< 11){ 
dcs3->setRot(((z- 42 *(i - l))/3.0),0 ,0); 
dcs6->setRot(((z- 42 *(i - l))/3.0),0 ,0); 

} 

else{ 

dcs3->setRot(((20 - (z- 42 *(i - l)))/3.0),0 ,0); 
dcs6->setRot(((20 - (z- 42 *(i - l)))/3.0),0 ,0); 

} 



dcsO->setTrans( X + 1 .0 + z * 0. 1 , Y , Z + 0.2 + O.OSf 
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*cosj;2.0f * 3. 14159f * z / 21.0f)); 



UpdateViewO; 

UpdateGUIO; 

pfFrameO; 

) 

for ( z = (i-l)*42 + 22 ; z < (i-l)*42 + 43 ; z += 0.5 * delta_t){ 
/* Go to sleep until next frame time. */ 
pfSyncQ; 

Shared->simTime = pfGetTime(); 

/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1,0.005); 

/* Left foot */ 
dcs9->setS cale(0.7 f) ; 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScaile(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 ,-1.7,0); 

/* Left lowerleg */ 
dcs4->setTrans(0 ,-1.7, 0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1,7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 
dcsl l->setTrans(-1.0 , 1.7,0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 
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/* Left upper arm */ 

dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6 , 1.8,0); 



/* Left lower arm */ 
dcsl5->setRot(0, 0 , 180.0); 
dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

lllllllllllllllllllllllllllllllllllllllllllllllllllin 

H Inverse Kinematics 

lllllllflllllllllllllllllllllllllllllllllllllinillllHI 



iiiiiiHiiiniiiiiiiininiiiiiiiiiiiiiiiiniiiniHiin 
n SECOND STEP 
// RIGHT LEG STEP 

x_right = 0.95f * cos((64 - 2* (z- 42 *(i - 1))) * DEG.TO^RAD ) 

* (11 + 12) * cos((64 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD); 
y.right = 0.95f * cos((64 - 2* (z- 42 *(i - 1))) * DEG.TO^RAD ) 

* (11 + 12) * sin((64 - 2* (z- 42 *(i - 1))) * DEG_TO_RAD); 

cl_right= ((x_right*x_right) + (y_right*y_right) 

- (11*11) -(12*12))/(2* 11 *12); 
theta2 = (1 *acos(cl fright)); 

kl fright = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

theta 1 = atan(y_right/x_right) - atan(k2_right/kl_right); 

// Right Leg 

dcs2->setRot(0 ,(57.3 * thetal),0 ); 
dcsl->setRot(C ,(57.3 * theta2), 0); 

// Left ARM 

dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0); 
dcsl5->setRot(0 ,(1.5f *(57.3 * thetal) - 10.0f),0); 

// LEFT LEG STEP 

xjeft = cos((-64 + 2* (z- 42 *(i - 1))) * DEG.TO.RAD ) 

* (11 + 12) * cos((-64 + 2* (z- 42 *(i - 1))) * DEG_TO_RAD); 
yjeft = cos((-64 + 2* (z- 42 *(i - 1))) * DEG_TO_RAD ) 

* (11 + 12) * sin((-64 + 2* (z- 42 *(i - 1))) * DEG.TO^RAD); 

cl_left= ((xjeft*xjeft) + (yjeft*y_left) - (11*11) 

-(12*12))/(2*11 *12); 
theta2 = (l *acos(cl Jeft)); 

kljeft = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

theta L= atan(y_left/x_left) - atan(k2_left/kl Jeft); 
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dcs5->setRot(0 ,(57.3 * theta 1), 0); 
dcs4->setRot(0 ,(57.3 * theta2), 0); 

// Right arm 

dcsll->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcsl2->setRot(0 ,(1.5f *(57.3 * thetal) - 10.00, 0); 

// Torso rotation 
if((z-42*(i- 1))<31){ 
dcs3->setRot((-(z- 42 *(i - 1) - 21)/3.0),0 ,0); 
dcs6->setRot((-(z> 42 *(i - 1) -21)/3.0),0 ,0); 

} 

else{ 

dcs3->setRot(((-20 + ((z- 42 *(i - 1)- 21)))/3.0),0 ,0); 
dcs6->setRot(((-20 + ((z- 42 *(i - 1)- 21)))/3.0),0 ,0); 

1 

dcsO->setTrans(X + 1 .0 + z * 0. 1 , Y , Z + 0.2 + 0.08f 
* cos(2.0f * 3.14159f * z / 21.00); 
UpdateViewO; 

UpdateGUIO; 

pfFrameO; 

} 

}//Big FOR 

// LAST HALF STEP ///////// 

for ( z = 1 ; z < 12 ; z += 0.5 * delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->si mTi me=pfGetTi me(); 

/* Main Body */ 

dcs0->setRot(90.0, 90.0 , 0); 
dcsO->setTrans(X,Y,Z); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.00 ; 
dcs6->setTrans(0.3 ,2.1, -0.005); 
dcs3->setScale(2.00; 
dcs3->setTrans(0.3 , 2.1 , 0.005); 

/* Left foot */ 
dcs9->setScale(0.70; 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setSc2ile(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
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dcs8->setTran$(0 ,-1.7,0); 



/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 
dcsl l->setTrans(-1.0 , 1.7, 0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcs 14->setScale(0.25f); 
dcsl4->setTrans(1.6 ,1.8,0); 

/* Left lower arm */ 
dcsl5->setRot(0, 0 , 180.0); 
dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

lllllllllllllllllllllllllllllllllllllllllllllllllllllU 
U Inverse Kinematics 

illlllllllllllllllllllllllllllllllllllllllllllllllllllll 
I I LEFT LEG 

xjeft = 0.95f * cos((22 - 2* z) * DEG.TO^RAD )*(!!+ 12) 

* cos((22 - 2* z) * DEG_TO_RAD); 

y.left = 0.95f * cos((22 - 2* z) * DEG_TO_RAD ) *(11 + 12) 

* sin((22 - 2* z) * DEG_TO^RAD); 

cl_left= ((x_left*xjeft) + (yjeft*y_left) 

- (11*11) -(12*12))/(2* 11 *12); 

theta2 = (1 *acos(cl Jeft)); 

kl Jeft = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

theta 1 = atan(y_left/x_left) - atan(k2_left/kl_left); 

dcs5->setRot(0 ,(57.3 * thetal),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 
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// Right arm 

dcsl l->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcsl2->setRot(0 ,(1.5f *(57.3 * thetal) - lO.Of), 0); 

// RIGHT LEG 

x^right = cos((-22 + 2 * z) * DEG_TO_RAD ) * (11 + 12) 

* cos((-22 + 2 * z) * DEG_TO_RAD); 

y.right = cos((-22 + 2 * z) * DEG_TO_RAD ) * (11 + 12) 

* sin((-22 + 2 * z) * DEG^TO_RAD); 
cl_right= ((x_right*x_right) + (y_right*y_right) - (11 *11) 

-(12*12))/(2*11 *12); 
theta2 = (l *acos(cl_right)); 

kl_right = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

thetal = atan(y_right/x_right) - atan(k2_right/kl_right); 

dcs2->setRot(0 »(57.3 * thetal), 0); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 

// Left arm 

dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcsl5->setRot(0 ,(1.5f *(57.3 * thetal) - 10.00, 0); 

// TORSO rotation 
if(z< 11){ 

dcs3->setRot((z/3.0),0 ,0); 
dcs6->setRot((z/3.0),0 ,0); 

} 

else{ 

dcs3->setRot(((20 - z)/3.0),0 ,0); 
dcs6->setRot(((20 - z)/3.0),0 ,0); 

} 

dcsO->setTrans(X + 1.0 + (i-1) * 4.2 + z * 0.1 

, Y , +Z + 0.2 + O.OSf *cos(2.0f * 3.14159f * z / 21 .00); 

UpdateViewO; 

UpdateGUIO; 

pfFrameO; 

}// END of step_forward 

} 
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II 

// Function: 

// Returns: 

// Parameters: 
// Summar>': 

// 

// 

// 

// 

// 

n 



step_upward 

None 

Number of steps needs to be taken, initial position 
Computes the joint angles according to the position 
of the end effector(foot) by using the inverse kinematic 
equations of the three link planar manipulator. The 
algorithm for the path of the foot is described in 
Chapter IV of this thesis as “Stepping Upward Algorithm. 
Height of each step is 0.267949 units. 



void step_upward(int number_of_steps,float X,float Y, float Z){ 



float 11 = l.Of; 
float 12= l.Of; 
float cl_left,cl_right; 
float theta l,theta2; 
float kl_left,k2_left; 
float kl_right,k2_right; 

float xjeft= 1.732f; 
float y_left = O.Of; 
float x_right = 1.732f; 
float y_right = O.Of; 
float z; 

lllllllllllllllllllllllllllllllllllll 
n FIRST HALF STEP 
lllllllllllllllllllllllllllllllllll 
for ( 2 = 1 1 .0 ; 2 < 22.0 ; z += 0.3 * delta_t) 
{ 



/* Go to sleep until next frame time. */ 
pfSyncQ; 

Shared->simTime=pfGetTime(); 

/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0.0); 
dcsO->setTrans(X,Y,Z); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1,0.005); 



/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 1 80.0); 
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dcs9->setTrans(0 ,-1.7,0); 



/* Right foot */ 

dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 ,-1.7,0); 

/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcs 1 1 ->setScale(0.250; 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcs 1 4->setScale(0.25f); 

//dcsl4->setTrans(1.6 ,1.8,0); 

/* Left lower arm */ 
dcsl5->setRot(0, 0 , 180.0); 
dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

lllllllllllllllllllllllinilllllllllllllllllllllllllin 

// Inverse Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
n FIRST STEP 
// LEFT LEG 

xjeft = 0.95f * cos((22 - 2* z - 10) * DEG_TO_RAD ) *(11 -h 12) 

* cos((22 - 2* z - 10) * DEG_TO_RAD); 

yjeft = 0.95f * cos((22 - 2* z - 10) * DEG_TO_RAD )*(!!+ 12) 

* sin((22 - 2* z - 10) * DEG_TO_RAD); 

cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 

- (12*12))/(2 * 11 *12); 

theta2 = (l *acos(cl_left)); 

kl_left = 11 -H (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

theta 1 = atan(y_left/x_left) - atan(k2_left/kl Jeft); 
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dcs5->setRot(0 ,(57.3 * theta 1),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 

// Right arm 

dcsl l->setTrans(-1.0 , 1.7, 0.5); 

dcsl l->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 

dcsl2->setRot(0 ,(1.5f *(57.3 * thetal) - lO.Of), 0); 

// RIGHT LEG 

x.right = cos((-22 + 2 * z - 10) * DEG_TO_RAD ) * (11 + 12) 

* cos((-22 + 2 * z - 10) * DEG_TO_RAD); 

y_right = cos((-22 + 2 * z - 10) * DEG_TO_RAD )*(!!+ 12) 

* sin((-22 + 2 * z - 10) * DEG_TO_RAD); 

cl_right= ((x_right*x_right) + (y_right*y_right) - (11*11) 

-(12*12))/(2*11 *12); 
theta2 = (1 *acos(cl_right)); 

kl.right = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

thetal = atan(y_right/x_right) - atan(k2_right/kl_right); 

dcs2->setRot(0 ,(57.3 * thetal), 0); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 

// Left arm 

dcsl4->setTrans(1.6 ,1.8, 0.5); 
dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcsl5->setRot(0 ,(1.5f *(57.3 * thetal) - lO.Of), 0); 

// TORSO rotation 
if(z < 1 1){ 

dcs3->setTrans(0.3 ,2.1,0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot((z/3.0),7.0,0); 
dcs6->setRot((z/3.0),7.0,0); 

} 

else{ 

dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 ,2.1, 0.305); 
dcs3->setRot(((20 - z)/3.0) ,7.0,0); 
dcs6->setRot(((20 - z)/3.0) ,7.0,0); 

} 

z = Z+2T, 

dcs0->setTrans( X + (z - 32) * 0.1 -0.4, Y , 

Z - 2.005063 + /* 0.08f *cos(2.0f * 3. 141 59f * z / 21 .Of) */0 
+ 1 * 0.267949 * 3.4 *2 + ((11 + 12) 

-(11 + 12) * cos(( -22 + 2 * z - 10) * DEG_TO_RAD))); 
z = z-21; 



UpdateViewO; 
UpdateGUIO; 
pfFrameO; 
}//END FOR 
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for( i = 1 ; i < number_of_steps ; i++){ 



for ( 2 = (i-l)*42 + 22.0 ; z < (i-l)*42 + 43.0 ; z += 0.35 * delta_t){ 
/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime = pfGetTime(); 

/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0.0); 
dcsO->setTrans(X,Y,Z); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.00; 

dcs3->setTrans(0.3 ,2.1,0.005); 

/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 

/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcs 1 4->setScale(0.25f); 

/* Left lower arm */ 
dcs 1 5_->setRot(0, 0 , 1 80.0); 
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dcsl5->setTrans(-0.5 , -4.6, 0.0); 



/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

II Inverse Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 



lllllllllllllllllllllllllllllllllllllllllllllllllllllllllin 

n SECOND STEP 
// RIGHT LEG STEP 

x. right = 0.95f * cos((64 - 2* (z- 42 *(i - 1)) - 10) * DEG.TO.RAD) 

* (11 + 12) * cos((64 - 2* (z- 42 *(i - 1)) - 10) 

* DEG_TO_RAD); 

y. right = 0.95f * cos((64 - 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD) 

* (11 + 12) * sin((64 - 2* (z- 42 *(i - 1)) - 10) 

* DEG_TO_RAD); 

cl_right= ((x_right*x_right) + (y_right*y_right) - (11*11) 
-( 12 * 12 ))/( 2*11 * 12 ); 
theta2 = (1 *acos (cl fright)); 

kl_right = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

theta 1 = atan(y_right/x_right) - atan(k2_right/kl_right); 

// Right LEG 

dcs2->setRot(0 ,(57.3 * thetal),0 ); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 

// Left arm 

dcsl4->setTrans(1.6 , 1.8, 0.5); 
dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.00,0 ); 
dcsl5->setRot(0 ,(1.5f *(57.3 * thetal) - 10.00, 0); 



// LEFT LEG STEP 

x_left = cos((-64 + 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD ) 
* ( 11 + 12 ) 

* cos((-64 + 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD); 
yjeft = cos((-64 + 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD ) 

* (11 + 12 ) 

* sin((-64 + 2* (z- 42 *(i - 1)) - 10) * DEG.TO.RAD); 

cl_left= ((x_left*x_left) + (yjeft*y_left) - (11*11) 
-( 12 * 12 ))/( 2*11 * 12 ); 
theta2 = (1 *acos (deleft)); 

kljeft = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

thetal = atan(y_left/x_left) - atan(k2_left/kl_left); 
dcs5->setRot(0 ,(57.3 * thetal), 0); 
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dcs4->setRot(0 ,(57.3 * theta2), 0); 



// Right arm 

dcsl l->setTrans(-1.0 , 1.7, 0.5); 

dcsl l->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 

dcsl2->setRot(0 ,(1.5f *(57.3 * thetal) - lO.Of), 0); 

// TORSO rotation 
if((z- 42*(i- 1))<31){ 
dcs3->setTrans(0,3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 , 2,1 , 0.305); 
dcs3->setRot((-(z- 42 *(i - 1) - 21)/3.0) ,7.0,0); 
dcs6->setRot((.(z- 42 *(i - 1) -21)/3.0) ,7.0,0); 

1 

else{ 

dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 ,2.1,0.305); 
dcs3->setRot(((-20 + ((z- 42 *(i - 1)- 21)))/3.0) ,7.0,0); 
dcs6->setRot(((-20 + ((z- 42 *(i - 1)- 21)))/3.0) ,7.0,0); 



z = z-2 1 ; 

dcs0->setTrans( X + 0,5 + z * 0.1 , Y , Z 
+ i * 0.267949 * 3.4 *2 - ((11 + 12) - (11 + 12) 

* cos(( 64 - 2 * (z« 42 *(i - 1)) ) * DEG_TO_RAD))); 
z = z+21; 



UpdateViewO; 

UpdateGUIO; 

pfFrameO; 



for ( z = (i-l)*42 + 1.0 ; z < (i-l)*42 + 22.0 ; z += 0.35 * delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime=pfGetTime(); 

/* Main Body */ 

dcs0->setRot(90.0, 90.0 ,0); 
dcsO->setTrans(X,Y,Z); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2,1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1,0.005); 
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/* Left foot */ 

dcs9->setScaIe(0.7O; 
dcs9->setRot(0, 0 . 180.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScale(0.70; 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 ,-1.7,0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 

dcs 1 4->setScale(0.25f); 

/* Left lower arm */ 
dcsl5->setRot(0, 0 , 180.0); 
dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

llllllllllllllllllllllllinilllllllllllllllllinillH 

I I Inverse Kinematics 

llllllllllllllllllllllllllllllllllllllllllllllllllllllll 
U FIRST STEP 
// LEFT LEG 

xjeft = 0.95f * cos((22 - 2* (z- 42 *(i - D) - 10) * DEG_TO_RAD ) 
*(11 -h 12) * cos((22 - 2* (z- 42 *(i - 1)) - 10) *DEG^TO^RAD); 
yjeft = 0.95f * cos((22 - 2* (z- 42 *(i - 1)) - 10) * DEG_TO_RAD ) 
*(11 + 12) * sin((22 - 2* (z- 42 *(i - 1)) - 10) *DEG_TO^RAD); 
cl_left= ((x_left*x_left) -h (y_left*y_left) - (11*11) 

-(12*12))/(2*11 *12); 

theta2 = (l *acos(cl_left)); 

kl Jeft = 11 -H (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 
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theta 1 - atan(y_left/x_left) - atan(k2_left/kl_left); 

dcs5->setRot(0 ,(57.3 * thetal),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 

// Right arm 

dcsll->setTrans(-1.0 , 1.7, 0.5); 

dcsl l->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 

dcsl2->setRot(0 ,(1.5f *(57.3 * thetal) - lO.Of), 0); 

// RIGHT LEG 

x. right = cos((-22 + 2 * (z- 42 *(i - 1)) - 10) * DEG_TO_RAD ) 

* (11 + 12) * cos((-22 + 2 * (z- 42 *(i - 1)) - 10) 

* DEG_TO_RAD); 

y. right = cos((-22 + 2 * (z- 42 *(i - 1)) - 10) * DEG.TO^RAD ) 

* (11 + 12) * sin((-22 + 2 * (z- 42 *(i - 1 )) - 10) 

* DEG_TO_RAD); 

cl_right= ((x_right*x_right) + (y_right*y_right) • (11*11) 
-(12*12))/(2*11 *12); 
theta2 = (l *acos(cl_right)); 

kl_right = 11 + (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

thetal = atan(y_right/x_right) - atan(k2_right/kl_right); 

dcs2->setRot(0 ,(57.3 * thetal), 0); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 

// Left arm 

dcsl4->setTrans(1.6 ,1.8, 0.5); 
dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.0f),0 ); 
dcsl5->setRot(0 ,(1.5f *(57.3 * thetal) - lO.Of), 0); 

// TORSO rotation 
if((z- 42 *(i- 1))< 11){ 
dcs3->setTrans(0.3 , 2.1 , 0.305); 
dcs6->setTrans(0.3 ,2.1,0.305); 
dcs3->setRot(((z- 42 *(i - l))/3.0) ,7.0 ,0); 
dcs6->setRot(((z- 42 *(i - l))/3.0) ,7.0 ,0); 

} 

else{ 

dcs3->setTrans(0.3 ,2.1,0.305); 
dcs6->setTrans(0.3 , 2.1 , 0.305); 
dcs3->setRot(((20 - (z- 42 *(i - l)))/3.0) ,7.0 ,0); 
dcs6->setRot(((20 - (z- 42 *(i - l)))/3.0) ,7.0 ,0); 

) 

z = z+21; 

dcsO->setTrans(X + 0.5 + z * 0.1 , Y , Z -0.105327 

+ i * 0.267949 * 3.4 *2 + ((11 + 12) -(11 + 12) 

* cos(( -22 + 2 * (z- 42 *(i - 1)) - 10) 

* DEG_TO_RAD))); 

z = z-21; 

UpdateViewO; 

UpdateGUIO; 

pfFrameO; 

}//END FOR 
}//BigFOR^ 
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// LAST HALF 

for ( z = 22.0 ; z < 33.0 ; z += 0.35 * delta^t){ 
/* Go to sleep until next frame time. */ 
pfSyncQ; 

Shared->simTime = pfGetTimeQ; 

/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0.0); 
dcsO->setTrans(X,Y,Z); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 , 2.1 , -0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1,0.005); 



/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 ,-1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 ,-1.7,0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.250; 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm ’*'/ 
dcs 14->setScale(0.250; 



/* Left lower arm */ 
dcsl5->setRot(0, 0 , 180.0); 
dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

lllllllllllllllllllllllllinilllllllllllllllllllllin 

H Inverse Kinematics 

lllinilllllllllllllllllllllllllllllllllllllllllllllllll 



IllllllllllllllllllllllllllinillllllllllllllllllllUIII 
Ji SECOND STEP 
// RIGHT LEG STEP 

x. right = 0.95f * cos((64 - 2* z - 10) * DEG_TO_RAD )* (11 + 12) 

* cos((64 - 2* z - 10) * DEG_TO_RAD); 

y. right = 0.95f * cos((64 - 2* z - 10) * DEG_TO_RAD )* (11 + 12) 

* sin((64 - 2* z - 10) * DEG_TO_RAD); 

cl_right= ((x_right*x_right) + (y_right*y_right) - (11*11) 

- (12*12))/(2 * 11 *12); 
theta2 = (1 *acos(cl_right)); 

kl_right = 11 -f (12 * cos(theta2)); 
k2_right = 12 * sin(theta2); 

theta 1 = atan(y_right/x_right) - atan(k2_right/kl_right); 

// Right LEG 

dcs2->setRot(0 ,(57.3 * theta 1),0 ); 
dcsl->setRot(0 ,(57.3 * theta2), 0); 

// Left arm 

dcsl4->setTrans(1.6 ,1.8, 0.5); 
dcsl4->setRot(0 ,(0.7f *(57.3 * thetal) + 10.00,0 ); 
dcsl5->setRot(0 ,(1.5f *(57.3 * thetal) - 10.00, 0); 

// LEFT LEG STEP 

xjeft = cos((-64 + 2* z - 10) * DEG_TO_RAD ) * (11 -k 12) 

* cos((-64 -H 2* z - 10) * DEG_TO_RAD); 

y_left = cos((-64 + 2* z - 10) * DEG_TO_RAD ) * (11 + 12) 

* sin((-64 -H 2* z - 10) * DEG_TO_RAD); 

cl_left= ((xjeft*xjeft) + (yjeft*yjeft) - (11*11) 

- (12*12))/(2 * 11 *12); 
theta2 = (1 *acos(cl_left)); 



kljeft = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

thetal = atan(y_left/xjeft) - atan(k2_left/kl Jeft); 
dcs5->setRot(0 ,(57.3 * thetal), 0); 
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dcs4->setRot(0 ,(57.3 * theta2), 0); 

// Right arm 

dcsl l->setTrans(-1.0 , 1.7, 0.5); 

dcsl l->setRot(0 ,(0.7f *(57.3 * thetal) + 10.00,0 ); 

dcsl2->selRot(0 ,(1.5f *(57.3 * thetal) - 10.00, 0); 

// Torso rotation 
if(z<31){ 

dcs3->setTrans(0.3 ,2.1,0.305); 
dcs6->setTrans(0.3 ,2.1,0.305); 
dcs3->setRot((-z - 21)/3.0 +13.75,7.0,0); 
dcs6->setRot((-z -21)/3.0 +13.75,7.0,0); 

} 

else{ 

dcs3->setTrans(0.3 ,2.1,0.305); 
dcs6->setTrans(0.3 ,2.1,0.305); 
dcs3->setRot((-20+ z- 21)/3.0 ,7.0,0); 
dcs6->setRot((-20 + z- 21)/3.0 ,7.0,0); 

} 

z = z-2 1 ; 

dcsO->setTrans( X + 0.5 +(i-l) * 4.2+ z * 0.1 , Y , Z 

+ number_of_steps * 0.267949 * 3.4 *2 - ((11 + 12) 
- (11 + 12) * cos(( 64 - 2 * z ) * DEG_TO_RAD))); 



z = z+21; 

Update View(); 

UpdateGUIQ; 

pfFrameO; 

}//End of for 
}//End of step_upward 



147 



' Function: 


jump 


' Returns: 


None 


' Parameters: 


Initial position 


' Summary: 


Translate the whole body first straight upward, 
secondly along a semi circle path, then straight down 
and straight up to an upright position. While translating 
the body appropriate joint angles are applied. 



void 

jump(float X, float Y,float Z){ 

float 11 =: l.Of; 
float 12= LOf; 
float cl Jeft,cl_right; 
float theta l,theta2; 
float kl_left,k2_left; 
float kl_right,k2_right; 

float x_left= 1.732f; 
float yjeft = O.Of; 
float x_right= 1.732f; 
float y_right = O.Of; 

for (float j = 1 ; j < 101 ; j += delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncQ; 

Shared->simTime = pfGetTimeO; 

/* Main Body */ 
dcs0->setRot(90.0, 90.0 , 0); 
dcs0->setXrans(X+j*0.8/100.0,Y,Z-j * 1.5/100.0); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 

dcs6->setScale(2.00; 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1,0.005); 
dcs3->setRot(0, 10.0*j/100.0 , 0.0); 
dcs6->setRot(0, 10.0*j/100.0 , 0.0); 

x_left= 1.95- 1.0 *j/l 00.0; 
y_left = 0.0; 

cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 
-( 12 * 12 ))/( 2*11 * 12 ); 

theta2 = (l *acos(cl Jeft)); 

kljeft = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 
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theta 1 = atan(y_left/x_left) - atan(k2_left/kl_left); 



dc$5->setRot(0 ,(57.3 * theta 1),0 ); 
dcs4->setRot(0 ,(57.3 * theta2) , 0); 
dcs2->setRot(0 ,(57.3 * thetal),0 ); 
dcsl->setRot(0 ,(57.3 * theta2) , 0); 

/* Left foot */ 
dcs9->setScale(0.70; 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 

/* Right foot */ 
dcs8->setScale(0.70; 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 ,-1.7,0); 

/* Left upperleg */ 
dcs5->setTran$(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 

/* Right upper arm */ 
dcsl l->setScale(0.250; 
dcsll->setTrans(-1.0 , 1.7, 0); 
dcsll->setRot(0.0,-j/5.0 ,0.0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 
dcsl2->setRot(0.0 ,-j/5.0,0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcsl4->setScale(0.250; 
dcsl4->setTrans(1.6 , 1.8,0); 
dcsl4->setRot(0.0 ,-j/5. 0,0.0); 

/* Left lower arm */ 
dcsl5->setRot(0,-j/5.0, 180.0); 
dcsl5->setTran$(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

UpdateViewO; 

UpdateGUIO; 

pfFrameO; 
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for (float k = 1 ; k < 101 ; k += 5.0 * delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime = pfGetTimeO; 

xjeft = 0.95 + 0.5 *k/100.0; 
yjeft = 0.0; 

/* Main Body */ 

dcs0->setRot(90.0 , 90.0 + 1 5.0* k/1 00.0 , 0); 
dcsO->setTrans(X+ 100.0*0.8/100.0 + (x_left* 1.7 
* sin(15.0*k*DEG_TO_RAD/100.0)), Y , 

Z- 100.0* 1.5/100.0 + 0.95 *k/100.0-x_left* 1.7 
*(1.0 - cos(15.0*k*DEG_TO_RAD/100.0))); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1, 0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 

cl_left= ((x_left*x_left) + (yjeft*yjeft) - (11*11) 
-(12*12))/(2*11 *12); 

theta2 = (1 *acos(cl_left)); 

kljeft = 11 + (12 * cos(theta2)); 
k2Jeft = 12 * sin(theta2); 

theta 1 = atan(y_left/x_left) - atan(k2_left/kl_left); 

dcs5->setRot(0 ,(57.3 * theta 1) ,0); 
dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(0 ,(57.3 * thetal) ,0); 
dcsl->setRot(0 ,(57.3 * theta2) ,0); 

/* Left foot */ 

dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , - 1 .7, 0); 

/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 1 80.0); 
dcs8->setTrans(0 ,-1.7,0); 

/* Left lowerleg */ 
dcs4->setTrans(0 ,-1.7,0); 

/* Left upperleg */ 
dcs5->5etTrans(0.6, 0, 0); 
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/* Right lowerleg */ 
dcs 1 ->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 
dcs 1 1 ->setTrans(- 1.0,1 .7, 0); 
dcsl l->setRot(0.0,-j/5.0 ,0.0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 
dcsl2->setRot(0.0 ,-j/5.0,0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0, 180.0); 

/* Left upper arm */ 

dcs 14->setScale(0.250; 
dcsl4->setTrans(1.6 ,1.8,0); 
dcsl4->setRot(0.0 ,-j/5. 0,0.0); 

/* Left lower arm */ 
dcsl5->setRot(0,-j/5.0, 180.0); 
dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

Update View(); 

UpdateGUIO; 

pfFrameO; 

} 

for (float 1=1 ; 1 < 101 ; 1 += delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime = pfGetTime(); 

x_left= 1.45; 
y Jeft = 0.0; 

/* Main Body */ 

dcs0->setRot(90.0 , 90.0+ 360.0*1/100.0 , 0); 

dcs0->setTrans(3.0 + 3.0*cos(( 180.0 - 180.0* 1/1 00.0) *DEG_TO_RAD)+ X 
+ 100.0*0.8/100.0 +(x_left* 1.7 

* sin(15.0*100.0*DEG_TO_RAD/100.0)),Y , 

3.0*sin((180.0- 180.0* 1/100.0)*DEG_TO_RAD) 

+ Z- 100.0 * 1.5/100.0+ 0.95 * 100.0/100.0 - xjeft 

* 1 .7*(1 .0 - cos(l 5.0* 100.0*DEG_TO_RAD/100.0))); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 
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/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1,0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 

cl_left= ((x_left*x_left) -i- (y_left*y_left) - (11*11) 
- (12*12))/(2 * 11 *12); 

theta2 = (l *acos(cl_left)); 

kl Jeft = 11 + (12 * cos(theta2)); 

k2_left = 12 * sin(theta2); 

thetal = atan(y_left/x_left) - atan(k2_left/kl_left); 

dcs5->setRot(0 ,(57.3 * thetal) ,0); 
dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(0 ,(57.3 * thetal) ,0); 
dcsl->setRot(0 ,(57.3 * theta2) ,0); 

/* Left foot */ 
dcs9->setScale(0.70; 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 , -1.7, 0); 

/* Right foot */ 
dcs8->setScale(0.70; 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 

/* Left upperleg */ 
dcs5->setTraris(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 

/* Right upper arm */ 
dcsl l->setScale(0.250; 
dcsl l->setTrans(-1.0 , 1.7, 0); 
dcsl l->setRot(0.0,- 100.0/5.0 ,0.0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 
dcs 1 2->setRot(0.0 ,- 1 00.0/5.0,0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 
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/* Left upper arm */ 
dcs 1 4->setScaIe(0.25f); 
dcsl4->setTrans(1.6 ,1.8,0); 
dcs 14->setRot(0.0 ,- 1 00.0/5.0,0.0); 

/* Left lower arm */ 

dcs 1 5>>setRot(0,- 1 00.0/5 .0, 1 80.0); 

dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

Update View(); 

UpdateGUIO; 

pfFrameO; 

} 

for (float m = 1 ; m < 101 ; m += 3.0 * delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime = pfGetTime(); 

x_left = 1.45; 
yjeft = 0.0; 

/* Main Body */ 

dcs0->setRot(90.0 , 90.0+ 360.0*100.0/100.0 , 0); 

dcs0->setTrans(3.0 + 3.0*cos(( 180.0 - 180.0* 1 00.0/1 00.0)*DEG_TO_RAD)+X 
+ 100.0*0.8/100.0 + (xjeft* 1.7 

* sin(15.0*100.0*DEG_TO^RAD/100.0)),Y , 

3.0*sin((180.0- 180.0* 1 00.0/100. 0)*DEG_TO_RAD) 

+ Z- 100.0 * 1.5/100.0 + 0.95 * 100.0/100.0 - xjeft 

* 1.7*(1.0 - cos(l 5.0* 100.0*DEG_TO_RAD/1 00.0)) 

-9.01*m/100.0); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f); 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 ,2.1,0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 

cl Jeft= ((xjeft*xjeft) + (yjeft*yjeft) - (11*11) 

-(12*12))/(2* 11 *12); 

theta2 = (l *acos(cl Jeft)); 

kljeft = 11 + (12 * cos(theta2)); 
k2Jeft = 12 * sin(theta2); 

theta 1 = atan(yjeft/xjeft) - atan(k2_left/kl Jeft); 
dcs5->setRot(0 ,(57.3 * thetal) ,0); 
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dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(0 ,(57.3 * thetal) ,0); 
dcsl->setRot(0 ,(57.3 * theta2) ,0); 

/* Left foot */ 
dcs9->setScale(0.70; 
dcs9->setRot(0, 0 , 1 80.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScale(0.70; 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setTrans(0 , -1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 ,-1.7,0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcsl->setTrans(0 ,-1.7,0); 

/* Right upper arm */ 
dcsl l->setScale(0.250; 
dcsl l->setTrans(-1.0 , 1.7, 0); 
dcsl l->setRot(0.0,- 100.0/5.0 ,0.0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 
dcsl2->setRot(0.0 ,-100.0/5.0,0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6 ,1.8,0); 
dcs 1 4->setRot(0.0 ,- 1 00.0/5.0,0.0); 

/* Left lower arm */ 

dcs 1 5->setRot(0,- 1 00.0/5.0, 1 80.0); 

dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

UpdateViewO; 

UpdateGUIO; 

pfFrameO; 

} 

for (float n = 1 ; n < 101 ; n += 5.0 * delta_t){ 
/* Go to sleep until next frame time. */ 
pfSyncO; 
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Shared->simTime = pfGeiTimeO; 



xjeft= 1.45 + 0.5 *n/100.0; 
y_left = 0.0; 

/* Main Body */ 

dcs0->setRot(90.0 , 90.0+ 360.0*100.0/100.0 , 0); 

dcs0->setTrans(3.0 + 3.0*cos(( 180.0 - 180.0* 100.0/100.0)*DEG_TO.RAD)+X 
+ 100.0*0.8/100.0 + (1.45* 1.7 

* sin(15.0*100.0*DEG_TO_RAD/100.0)),Y , 

3.0*sin((180.0- 180.0* 100.0/1 00.0) *DEG_TO.R AD) 

+ Z- 100.0 * 1.5/100.0 + 0.95 * 100.0/100.0- 1.45 

* 1.7*(1.0 - cos(15.0*100.0*DEG_TO_RAD/100.0)) 
-9.01*100.0/100.0+ n* 1.8/100.0); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 

/* Torso */ 
dcs6->setScale(2.0f) ; 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.0f); 
dcs3->setTrans(0.3 , 2.1 , 0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 

cl_left= ((x_left*x_left) + (y_left*y_left) - (11*11) 

- (12*12))/(2 * 11 *12); 

theta2 = (l *acos(cl_left)); 

kljeft = 11 + (12 * cos(theta2)); 
k2_left = 12 * sin(theta2); 

theta 1 = atan(y_left/x_left) - atan(k2_left/kl_left); 

dcs5->setRot(0 ,(57.3 * thetal) ,0); 
dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(0 ,(57.3 * thetal) ,0); 
dcsl->setRot(0 ,(57.3 * theta2) ,0); 

/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 1 80.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScale(0.70; 
dcs8->setRot(0, 0 , 180.0); 
dcs8->setXrans(0 , -1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 , -1.7, 0); 

/* Left-upperleg */ 
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dcs5->setTrans(0.6, 0, 0); 



/* Right lowerleg */ 
dcsl->setTrans(0 , -1.7, 0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 
dcsl l->setTrans(-1.0 , 1.7, 0); 
dcsl l->setRot(0. 0,-1 00.0/5.0 ,0.0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 
dcsl 2->setRot(0.0 ,- 1 00.0/5.0,0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left upper arm */ 
dcsl4->setScale(0.25f); 
dcsl4->setTrans(1.6 ,1.8,0); 
dcsl4->setRot(0.0 ,-100.0/5.0,0.0); 

/* Left lower arm */ 

dcs 1 5->setRot(0,- 1 00.0/5.0, 1 80.0) ; 

dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

UpdateViewO; 

UpdateGUIO; 

pfFrameO; 

} 

for (float 0 = 1 ; 0 < 101 ; o += 2.0 * delta_t){ 

/* Go to sleep until next frame time. */ 
pfSyncO; 

Shared->simTime = pfGetTime(); 

xjeft = 1.45 + 0.5 *100.0/100.0; 
y_left = 0.0; 

/* Main Body */ 

dcs0->setRot(90.0 , 90.0+ 360.0*100.0/100.0 , 0); 

dcs0->setTrans(3.0 + 3.0*cos((180.0 - 180.0* 1 00.0/1 00.0)*DEG_TO_RAD)+X 
+ 100.0*0.8/100.0 + (1.45* 1.7 

* sin(15.0*100.0*DEG_TO_RAD/100.0)),Y , 3.0 
*sin((180.0- 180.0* 100.0/100.0)*DEG^TO_RAD) 

+ Z- 100.0* 1.5/100.0 + 0.95 * 100.0/100.0- 1.45* 1.7 

* (1.0 - cos(l 5.0* 100.0*DEG_TO_R AD/1 00.0)) 

-9.01*100.0/100.0 + n* 1.8/100.0); 

/* Head */ 

dcs7->setTrans(0.0 , 0.4 , 0.0); 
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/* Torso */ 
dcs6->setScale(2.00; 
dcs6->setTrans(0.3 ,2.1,-0.005); 
dcs3->setScale(2.00; 
dcs3->setTrans(0.3 ,2.1,0.005); 
dcs3->setRot(0, 10.0 , 0.0); 
dcs6->setRot(0, 10.0 , 0.0); 

cl_left= ((x_left*xjeft) + (yjeft*y_left) - (11*11) 
-(12*12))/(2* 11 *12); 

theta2 = (1 *acos(cl_left)); 

kljeft = 11 + (12 * cos(theta2)); 
k2Jeft = 12 * sin(theta2); 

theta 1 = atan(yjeft/x_left) - atan(k2_left/kl_left); 

dcs5->setRot(0 ,(57.3 * thetal) ,0); 
dcs4->setRot(0 ,(57.3 * theta2) ,0); 
dcs2->setRot(0 ,(57.3 * thetal) ,0); 
dcsl->setRot(0 ,(57.3 * theta2) ,0); 

/* Left foot */ 
dcs9->setScale(0.7f); 
dcs9->setRot(0, 0 , 180.0); 
dcs9->setTrans(0 ,-1.7,0); 

/* Right foot */ 
dcs8->setScale(0.7f); 
dcs8->setRot(0, 0 , 1 80.0); 
dcs8->setTrans(0 ,-1.7, 0); 

/* Left lowerleg */ 
dcs4->setTrans(0 , - 1 .7, 0); 

/* Left upperleg */ 
dcs5->setTrans(0.6, 0, 0); 

/* Right lowerleg */ 
dcs 1 ->setTrans(0 ,-1.7, 0); 

/* Right upper arm */ 
dcsl l->setScale(0.25f); 
dcsl 1 ->setTrans(- 1 .0 ,1.7, 0); 
dcsl l->setRot(0.0,-l 00.0/5.0 ,0.0); 

/* Right lower arm */ 
dcsl2->setTrans(0.4 , -4.6, 0.0); 
dcs 1 2->setRot(0.0 ,- 1 00.0/5.0,0.0); 

/* Right hand */ 
dcsl3->setTrans(0.0 , -4.2, 0.0); 
dcsl3->setRot(0, 0 , 180.0); 

/* Left-upper arm */ 
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dc s 1 4->setScale(0. 25 f) ; 
dcsl4->setTrans(1.6 , 1.8,0); 
dcs 1 4->setRot(0.0 1 00.0/5 .0,0.0); 

/* Left lower arm */ 

dcs 1 5->setRot(0,- 1 00.0/5.0, 1 80.0); 

dcsl5->setTrans(-0.5 , -4.6, 0.0); 

/* Left hand */ 
dcsl6->setTrans(-0.2 , -4.6, 0.0); 

UpdateViewO; 

UpdateGUIO; 

pfFrameO; 
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